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ABSTRACT 



The technique of Extended Kalman filtering is applied 
to a passive single sensor . location and tracking problem. 
Signal angle of arrival and doppler shifted frequency are 
used as observations for two different filter formulations: 
one for fixed X-Y coordinates of position and velocity and 
one with an orthogonal system rotated to align an axis with 
the target heading. The fixed X-Y system was found to give 
better tracking performance on the Monte Carlo simulation. 

A new method of calculating the Initial covariance matrix 
was extended to reformulate the Kalman filter . equations for 
a nonlinear problem. The use of the full initial covariance 
matrix as opposed to only the diagonal elements, provides 
better transient response and lower error variances in the 



simulation . 
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I . INTRODUCTION 



There are many engineering problems in nonlinear filter- 
ing which do not have the luxury of many observations over 
an extended period of time. The estimator or filter is 
required to provide a rapid, accurate solution before steady 
state conditions of the gains and covariance of error are 
reached. In these situations it is imperative to use as 
much information as possible from the observations, the 
known apriori conditions, and the statistical relationship 
of the initial filter states . 

The location of acoustic emitters by remote passive 
sensors is such a problem. The received acoustic signals 
may be processed to obtain noisy angle of arrival and 
noisy doppler shifted frequency data. These, in turn, can 
be used as observables to provide location and tracking 
information on the emitting target. The ability to accom- 
plish this with a single sensor (sonobuoy) as well as the . 
flexibility to incorporate additional sensors as available 
would be highly desirable for the Anti-Submarine V/arfare 
Community. A filter using angle of arrival and doppler- 
shifted frequency can be applied to other tracking problems 
in the radar domain as v;ell. 

There are presently available methods which use only 
the doppler-shifted frequency from some array of three or 
more sensors [^6]. In general, they require a good initial 
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guess as to v;here the emitter is located because they rely 
on a linearization method closely resembling a least-square 
fit of the received frequency data. Currently, research 
is going on to incorporate angle and frequency data in a 
common arrangement [35]. 

A first objective of this research was to apply non- 
linear filtering theory to the single sensor location and 
tracking problem and develop an algorithm to solve this 
estimation problem. The calculation of initial filter 
states using a single sensor with angle and frequency measure- 
ments only, represents a, difficult task. A single sensor 
making either measurement alone does not provide a unique 
solution and it is only when the combination is used that 
a single determination of the target position is possible. 

The geometry of the problem, while simple, does not make 

determination of initial position using the specified obser- 
/ 

vatlons very simple. Yet, this initial position estimate 
must be chosen so that the subsequent linearised filter 
techniques will converge. In doing so, the effect of various 
choices which are available to the filter designer became 
evident. The primary choice is that of a system model, 
and even for a simple constant-velocity target, differences 
in the model formulation are possible. The' value of the 
initial filter states and their resultant uncertainty, as 
reflected in the initial covariance matrix, are seen to 
play important roles in the time it takes for the filter 
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to converge. Using all of the covariance terms in this 
matrix, as opposed to only the diagonal variances, results 
in better filter performance in the transient phase and 
lower error variances for a specified track duration. The 
geometric placement of sensors was considered to determine 
where they might be placed for best filter performance. 

It Is also Important to determine how well the filter is 
performing while In operation, not so much as a quantitative 
measure of accuracy, but whether the filter Is indeed esti- 
mating a reasonable track. Following the lead of Periot 
[3^], the filter residuals were found to be a good measure 
of a valid track. 

In Chapter Two general filtering theory is oonsidered 
with the emphasis on the underlying probability structure 
of the optimal filter. This approach clearly points out 
the simplicity of a linear-Gaussian system and its resultant 
filtering solution as well as the difficulties and complexi- 
ties of the nonlinear problem. The continuous formulation 
of the filtering problem is also reviewed with reference 
to stochastic integrals and the determination of the strictly 
formal solution. In the final section of Chapter Tv;o, the 
techniques that various authors have used to approximate 
the optimal nonlinear solution v/lll be presented. The 
approaches range from a direct calculation of the aposteriori 
density function [10], to a simple linearization of state 
and observation equations about some nominal state trajectoi’.v [3^ 
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Chapter Three describes in detail the single sensor 
passive location and tracking problem and will develop the 
two state formulations . The equations for an Extended 
Kalman Filter for each case are presented. The Important 
initial condition equations are derived and the initial 
covariance of error expressions for the states are found. 

The technique used to obtain the Initial covariance matrix 
is new and consists of a direct calculation of the changes 
in the nonlinear initialization equations when the input 
measurements change by one standard deviation. The values 
for the filter state excitation matrix (which turn out to be 
state dependent) i are presented next. The ability to add 
additional sensors or observables to the filter is an 
Important advantage of the Extended Kalman method and this 
is developed next. 

Chapter Pour contains a new approach to the determina- 
tion of error covariances for nonlinear functions of random 
variables. This approach is an extension of the method 
used to obtain the Initial covariance of state error matrix. 
The method may be readily applied to formulate the complete 
nonlinear filtering problem using the linear Kalman filter 
equations. The one step prediction and updating is accom- 
plished by a direct calculation of the change in the non- 
linear state or observation function caused by a one-standard 
deviation change in each state variable. The matrix of the 
correlation coefficients of the states is found to play an 
Important role . 
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A Monte Carlo simulation of the t.wo filters developed 
in Chapter Three is given in Chapter Five for a wide range 
of problem geometry. The effects of initial conditions 
and initial covariance matrices will be seen to play an 
important role in performance of the filter. 

The final chapter summarizes the results of this inves- 
tigation and presents the conclusions and suggestions for 
further study. 
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II. FILTERING THEORY 



A. THE PROBABILISTIC APPROACH FOR THE DISCRETE CASE 

Since the extraction of states or parameters from noisy 
measurements Is fundamentally a problem of estimating random 
variables. It Is important to consider the underlying prob- 
ability theory of a general filtering problem. Indeed the 
characterization of the estimates with the appropriate prob- 
cihility density function is the most complete knowledge that 
can be obtained about these estimates. The evolution of this 
density function in time, and its change with incoming measure- 
ments, represents the general solution to the estimation prob- 
lem. How the value for the estimate is determined will be 
governed by the cost or penalty charged for making a wrong 
choice [31j^^]. 

Assume a discrete system of the following form 

x(k+l) = f(x(k),k) + £(x(k) ,k) *w(k) (2.1) 

^(k) = h(x(k),k) + v(k) (2.2) 

with X the vector of states or parameters (or both) to be 
estimated and z the vector of observations. The notation used 
throughout the text is that vector quantities are lower case 
letters and underlined and matrices are given by upper case 
letters. The initial states x(.0) are assumed to be from some 
initial density p(x(0)) and the statistics of the system and 
measurement noise w and v arc known and given by 
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E[w(k)] = 0 
E[v(k)] = 0 

E[w(k)*v(J)] = 0 for all k,j 

E[£(x(k) ,k) ‘wCk) • (g(x(k) ,k) *w(k) )'^] = Q(k)5j^j 

E[v(k) •v'^(j ) ] = R(k)-6j^j 

with 

= 1 k=3 

= 0 k/3 , 

and 

E[w(k) •x'^(O) ] = 0 

E[v(k)*x'^(0)] = 0 . (2.3) 

Note that the above system of equations defines a Markov 
process; that is, a process whose probability law for future 
times depends only on the present state values. In terms of 
the density functions 

p (x(k+l )/x(k) (k-1 ) ... 2L^*^)) “ p (x (k+1 )/x(k) ) (2.4) 

The Markov property of a process is very fundamental and 
along with the uncorrelated noise assumptions written above, 
allows great simplification in the recursive expressions 
developed later. 

Let 

=■ {z(k),z(k-D,. . .,z(l)} (2.5) 

be the set of all measurements on the system up to the 
present time. The probabilistic knowledge one has about the 
system, given the set of measurements is contained in the 
conditional probability density function of the states given 
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the measurements: p(x(J)/Z^). Note that the Index value 

J and superscript k are not necessarily the same. This 
allows the three main classes of problems — smoothing for 
J < k, filtering for j = k, and prediction for j > k — to 
be considered simultaneously. 

1 . The Cost Function 

* 

Let the error of the estimate be e_( j ) = j ^ 

where 2L(J ) is the true state value at time t(J) and 2L(J ) is 
the value of the estimate. Let L(e(j)) be a cost function. 

The value of the estimate is chosen so as to minimize the 
expected value of the cost function: 

/\ 

x( j ) is the value of x(j) such that E[L(£(j))] is minimum, 
with 

E[L(e(j))] = / L(e(j))p(x(j)/Z^) dx( j ) . 

This expectation is taken over the conditional density 
function of the states and therein lies the importance of 
this function. 

Table I gives three main cost functions which lead 
to different choices for the estimate. 
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Cost Function L(e(J)) 



Name 



Estimate x(J) 



1. e^(j ) ‘e( j ) 



minimum variance Conditional 
or minimum mean mean 
square error 



2 . |^( j ) I 

3. = 0 e(j) < 6 

= 1 otherwise 



minimum absolute 
error 



uniform 



Conditional 

median 



Conditional mode 
or maximum 
a posteriori 
estimate 



TABLE I. Cost Functions and Their Estimates 



2 . Recursive Bayesian Formulation 

In the discrete formulation of the filtering problem, 
the conditional density function can be written in a recursive 
manner with the initial condition probability distribution 
as the starting function [26]. 

Let the conditional density of the filter states be 
p(x(k)/Z^). Using Bayes rule this can be written as: 



p(x(k)/Z^) = p(x(k)/z(k), Z^"^) 



p(x(k), ^(k), Z^”^) 
p(z(k) , Z^”^) 



p(z(k)/x(k), Z^ • p(x(k), Z^ h 
p(z(k)/Z^"^) • p(Z^"^) 



p(^(k)/x(k) ) 
p(z(k)/Z^“^) 



p(x(k)/Z^) 



• p(x(k)/Z^ 



( 2 . 7 ) 



Thus the current value of the conditional density function 
is found from the predicted density p(x(k) / Z^~^) and a 
weighting term which depends on the current observation. 

Since the denominator term does not depend on the states, 
x(k), it can be looked on as a normalizing constant[10] . 

The predicted conditional density function can be 
found from the density function one step back in the following 
manner : 



p(x(k) / Z^"^)= / p(x(k), x(k-l) / 

x(k-l) 

p(x(k) ,x(k-l) ,Z^“^) 

p(x(k)/Z ■^) = / ?— n dx(k-l) 

p(x(k)/x(k-l) ,Z^”^) p(x(k-l) ,Z^“^) ^x(k-l) 
x(k-l p(Z^“^) /p ON 



Now using the Markov property of the state system. 



p(x(k)/Z^ ^) = / 

x(k-l) 



p(x(k)/x(k-l) ) p(x(k-l)/Z^ dx(k-l) . 

( 2 . 9 ) 



The notation for the Integral indicates that the integration 
is performed over all permissible values o'f the states one 
step back. The predicted conditional density is thus found 
from the density one step back and involves a spatial 
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integration over all values of all the states at that time. 
The final recursive expression for the conditional density 
is thus 

p(x(k)/Z^) = 



p(£(k)/x(k) ) 
p(z(k)/Z^“^ 



/ 

x(k-l) 



p(x(k)/x(k-l)p(x(k-l)/Z^ dx(k-l) 

( 2 . 10 ) 



with the condition p(x(0)/Z°)= p(x(0)). The individual 
conditional density functions above are obtained from the 
probability density functions of the state excitation noise, 
w(k-l), and the observation noise, v(k) . 

Let the density of w(k) be p, (w(k)) and the density 
of v(k) be p^(v(k). Therefore 

p ( z (k) )/x (k) ) = p (^(k) - h( 2 c(k),k)) , (2.11) 

and 

p (x(k)/x(k-l) ) = P„[£ ^(x(k-l) ,k-l) • [x(k)-f (x(k-l) ,k-l) ]] 

( 2 . 12 ) 



3. Linear Gaussian Case 

When the state equations and the observation 
equations are linear, and the initial density of ^(0) and 
the densitjes of w(k-l) and v(k) are Gaussian all of the 
above conditional densities turn out to be Gaussian since 
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they involve linear operations on Gaussianly distributed - 
random variables. Since these densities are completely 
determined by two quantities, the vector mean and the 
covariance matrix, the recursive expression for the condi- 
tional density function, Eq . (2.10) simplifies to two 
coupled expressions: one for the vector mean and the other 

for the covariance matrix. These two expressions, when 
rev/ritten are the well-known Kalman filter equations [ 36 , 

22 , 23 ]. 

When the state or observation equations are nonlinear 
or the noise statistics are non-Gaussian, the above simpli- 
fication is not valid. Thus the entire density is needed 
to completely solve the problem. 

B. THE CONTINUOUS CASE 

The equations for the states and observations can be 
written in continuous form 

dx(t) = f(x(t),t)dt + £(x (t ) , t )dw(t ) , (2'.13) 



and 



d^(t) = h(x(t),t)dt + dv(t) . (2.1^) 

The above equations are Ito stochastic differential 
equations and must be manipulated usinp the rules of Ito 
calculus [ 17 ]. The difficulty with ordinary calculus is in 
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the definition of Integrals involving white noise. The 
first equation above is to be Interpreted as 



x(t) - x('tQ) 



t 

I f(x(t),t)dt + 



t 

/ g(x(t) ,t )dw(t ) . 

(2.15) 



Since the last Integral involves integration over random 

increments (dw(t)), with random quantities in the Integral, 

it is not even defined by ordinary calculus [21]. The 

increments appearing in Equations (2.13) and (2.1^), dw(t) 

and dv(t) are increments of two independent Wiener processes. 

When the continuous equations are interpreted in the 

Ito sense, the process x(t) still defines a Markov process 

% 

and as suc"h, evolution of the density function of x(t) is 

given by the Kolmogorov equations or the Fokker-Planck 

equation [19,^51. Kushner [25], Bucy [6], and Fisher [l8] 

have shown that the conditional density P(x(t)/Z^) - where 

is the set of observations in the Interval (t ,t) - also 

o ' 

obeys a Kolmogorov type equation which is modified by the 
observations. The result is an Ito differential equation . 
for the conditional density which is strictly a formal 
solution to the problem. By this is meant that no closed 
form solution can be found because there is no theory to 
solve the equation [21]. See also Bucy and Joseph [8]. 

C. SOLUTION METHODS AND APPROXIMATIONS 

It is no surprise, and this work is no exception, that 
most of tlie practical non-llne'ar filters use the Kalman 
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filter equations in some linearized system of equations 
[ 38 ]. The linearization is usually carried out about some 
nominal state trajectory or about a predicted value for the 
states on a step by step basis . This technique is Extended 
Kalman filtering. The tacit assumption here is that with 
Gaussian noise, there is some region in state space around 
the true state values where the system response is ..linear 
and therefore Gaussian. Similarly the observation errors 
will be linear and Gaussian. The greatest difficulty with 
this approach is insuring that the linearization is valid, 
especially at the initial filter estimates where the errors 
may be quite large. For the passive problem described in 
the next chapter, the initial conditions are not known and 
must be obtained from the first few observations with suffi- 
cient accuracy to insure filter convergence. 

Several authors have looked at higher order term.s in 
the expansions of the state and observations equations 
[3,5,27, ^0,^13]. This greatly Increases the complexity of 
the calculations and has not been shown to be generally 
applicable to all problems. 

There have been attempts to look at higher moments of 
the conditional density function Itself in an effort to 
characterize the function at each state point. Fisher [I 8 ] 
has derived equations for the evolution of higher order 
moments. The difficulty is in determining how many moments 
are needed to describe the density and in solving the highly 
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coupled set of nonlinear difference or differential equations 
v.'hlch are the result [24]. 

Srinivasan [4l] and Sorensen and Stubberuds [39] expanded 
the density into an othogonal series and then looked at the 
evolution of each of the terms in the expansion. It is 
difficult to know how many terms to Incorporate and in some 
cases the series may not converge at all [21]. Spline 
function expansions have been used but have the same inherent 
difficulties [l4]. Swirling, et al. [42] have attempted a 
logarithmic expansion of the conditional mean. 

One of the most successful expansion techniques has 
been the Gaussian-Sum Expansion of Alspach and Sorenson 
[1,2]. V/hile the computational burden is quite large, 
this method is one of the few that will solve multi-modal 
distributions. The primary reason for its universal appli- 
cation is that each linearization of the state and observa- 
tion equation only has to be valid about the mean of each 
of the assumed Gaussian distributions used in the sum. 

Each term in the sum is then filtered using the linear 
Kalman filter equations, and the results recombined and 
normalized to give an approximation to the conditional 
density. 

For limited order systems, Ducy and Senne [7] have done 
a direct calculation of the conditional density using the 
Bayes lav/ recursive equations developed in the previous sec- 
tion. Here again the computational burden is enormous and 
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Is only possible with two or three state variables. For 
these systems, however, this technique .does result In the 
complete solution to the nonlinear filtering problem In the 
sense that the result Is the complete aposterlorl condi- 
tional density function. The particular value of the state 
estimate may then be determined using a suitable cost cri- 
terion. See also Senne [37] and Bucy, Hecht and Senne [9]. 

The method of least squares, while being strictly a 
non-probablllstlc approach Is widely used In non-llnear 
problems [^,12,20,28,33]. In this method a non-probablllstlc 
cost function Is developed which Is usually a quadratic form 
of the state trajectory with some appropriate state model 
(See for example Sage and Melsa [36]). This cost function 
Is then minimized, using the observed data, with respect 
to the state trajectory points. As new data Is added a 
recursive algorithm can be used to update the estimates 
[ 15 , 30 ]. With appropriate weighting matrices used In the 
cost function, this method can be shown to be exactly equi- 
valent for Gaussian noise statistics to the maximum a pos- 
teriori estimate using a probability approach [21,36]. 

The method of least squares for nonlinear problems Is 
usually solved by linearization about some estimate and 
then Iteration until a solution Is found. Thus the Initial 
guess must be sufficiently close to allow for* convergence. 
Also for a large-order system the amount of computation may 
be extremely large. 
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The technique involves setting up a suitable cost 
function as a set of simultaneous non-linear equations 
which contains the observed data and some set of state 
parameters as unknowns. In this system of equations 
(usually over-determined) the "best fit" criteria is applied, 
generally with a data error norm. This method is used in 
the doppler-frequency-only localisation method [*^6]. When 
the system of over-determined equations is linear, this 
approach results in the pseudo-inverse matrix technique 
[28L 
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III. PASSIVE LOCATION AND TRACKING 



In the two-dimensional location problem, there are two 
position and two velocity components which will specify a 
constant-speed, constant-heading target. Since the dcppler 
shifted frequency is used as an observable, it is also 
necessary to estimate the rest frequency of the emitter. 
These five quantities will be used to define two different 
filter formulations: -one in which a target independent X-Y 
coordinate frame is used and one in which a target dependent 
system with one axis aligned with the estimated heading is 
used. Before presenting the two filter types, a general 
discussion of the Extended Kalman filter is given, 

A. EXTENDED KALMAN FILTER 

Consider a nonlinear discrete system of state and 
observation equations given by 

x(k+l) = f(x(k),k) + g(x(k) ,k) 'w(k) (3-1) 

and 

^(k) = h(x(k),k) + v(k) . ( 3 . 2 ) 

In these equations jT, £ and h are nonlinear functions of 
the state variables x, w(k) is plant excitation noise, and 
v(k) is measurement noise. The plant noise and measurement 
noise are assumed uncorrelated, sero-mean , and white. That 
is 
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and 



E[w(k) -w (j)] = Q'(k) 

•» 

E[v(k) v^(j)] = R(k) 

In order to apply the linear filter equations, (3.1) 
and (3.2) are expanded about the best estimate of the state 
at that time and only the first-order terms are kept. 
Equation (3.1) gives 



x(k+l) = $(k)x(k) + f(k) • w(k) 



with 



^•(k) 



9f 



x=x(k) 



Similarly Eq. (3.2) yields 



(3.3) 



z(k) = H(k) x(k) + v(k) 



( 3 .^) 



where 



H(k) 



ah 

¥x 



x=x' (k) 



(3.^^a) 



x(k) is the estimated state value after the k measui’ement 
and X* (k) is the predicted value of the state before the 

JU y. 

k*' measurement. That is, 

X' (k) - f (x(k-l) ,k-l) . 



3 ^ 



A state error vectbr Is defined by 



x(k) = x(k) - x(k) 



and a predicted state error vector is defined by 



x' (k) = x' (k) - x(k) 



The covariance of state error matrix is defined by 



P(k) = E[x(k) • x"^(k)] 



and the predicted covariance of state error matrix is 
given by 



P'(k) ■= E[x*(k) • x'"^(k)] 



The state excitation matrix is given by 



Q(k) = E[r(k)*w(k) • w'^(k) 



r^(k)] 



/ 




and the measurement noise covariance matrix is 



R(k) = E[v(k) • v'^(k)] 
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The Kalman Filter equations are given by [ 38 ] 





P’(k+1) =• $(k)P(k)$'^(k) + Q(k) 

G(k) = P' (k)H'’'(k) [H(k)P* (k)H^(k) + R(k)] 




P(k) = [I - G(k)H(k)] P' (k) 
x’(k) = f(x(k-l),k) 




z' (k) = h(2L* (k) ,k) 

x(k) = x' (k) + G(k)[_z(k) - _z’(k)] 



(3.5a) 

(3.5b) 

(3.5c) 

(3.5d) 

(3.5e) 

(3.5f) 



The Q matrix serves not only to allow for maneuvering 
but also to account for any model inaccuracies. That is, 
any discrepancies between the true action of the physical 
system and its characterization by Eq. (3.3). For a filter 
which reaches steady-state conditions the Q also serves to 
prevent the gain matrix G(k) from approaching zero by alv.^ays 
insuring uncertainty in the predicted covariance of error 
matrix P' (k) . 

1 . X-Y Filter Equations 

Figure 3.1 gives the geometry of the states used for 
the X-Y filter. For a constant-course, constant-speed target, 
the plant state equations can bo described as two second order 
systems, one for X and one for Y, and a state for the rest 
frequency, . These are 
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x(k+l) 



— 

x(k+l) 




x(k)+Tv^(k)+f^(y^ ,y^ ,k) 

s s 


v^(k+l) 




v^(k) + fj(Y5 

s s 


y(k+l) 




y(k)+Tv (k)+f +y* y ^k) 
y 3 ' 0 ^ ' V ^ 

s s 


Vy(k+1) 




Vy(k)+f^(y^ ,y; ,k) 

s s 


F^Ck+l) 

- — 




P^(k)+f.(y^ ) 

_ 



(3.6) 



where x(k), and y(k) are the position coordinates at time 
t(k), V (k) and v (k) are the X and Y components of velocity, 

t7 

T Is the time between observation, l.e., t(k+l) - t(k), and 

F^(k) Is the rest frequency of the emitter. 

The excitation terms f^ through f^ are Included to 

account for the random changes In speed and heading and 

which can occur for a maneuvering target. The quantities 

To 3 Y-,% are the random changes of the target. They 

®s 's ^o 

are assumed to be Independent, zero mean, plecewlse-constant 
rates of change. They have variances defined by 



a* ^ = E[y. 2] 
^s 



-6 ' = 

s s 



CTp ^ = E[y^, 23 
o o 
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The values for the standard deviations were taken from 
typical maneuvering parameters for the target; 



cr* == 100°/1000 sec 









\ p/ 

a* = 10 kts/1000 sec 



a-fn = .5 Hz/1000 sec. 
^o 









,0'/^ 






.0O-- ^ 



The effect of this excitation is to Increase the predicted 
value of the covariance of the state error matrix. 

Writing the equations in linear state form results 



in 



7 






X 



(k+1) = $ x(k) + r w(k) 'f' 



' 















■M-l 



(3.7) 



where 



$ = 



and 



r = 



1 

0 

0 

0 

0 



T 

1 

0 

0 

0 

r, 

2 

T 

0 

0 

0 



0 

0 

1 

0 

0 

UA/ 

? 

I 

1° 

T 

I 

0 

0 



0 

0 

T 

1 

0 



5_ 

2 

T 

0 



0 

0 

0 

0 

1 

0 

0 

0 

0 

T 



T'- 
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The vector w(k) represents tlie effect on the states of the 
random excitations and may be calculated from the equations 
relating target X and Y velocity to the target heading and 
velocity. The X velocity is 

X = V =-v *cos 0 (3.8) 

.X. o o 

which when differentiated gives 



X = Y • ' cos 0 
''s ® 



''s' si" ' ^I'v - • 

S S'^ S _ s 



(3.9) 



Similarly 



y = Vy = Vg‘Sin 0. 



( 3 . 10 ) 



and 



•• \ 

V = — ^ ‘Y • + V • Ya 

^ V ' V X ' 0 

s s s 



(3.11) 



The frequency term is just 




( 3 . 12 ) 



Thus 



w(k)'^ = [ 0 




V 

X 




V 



0 





V 



X 



Yp ] 

o 



(3.13) 



v/here from the assumption on the Y ' 



The excitation covariance matrix is thus found from 



Let 



Q = E[r w(k) w(k)'^ . 

''30^'} p 

a-2 = (!x) 2 a. 2 + V 2 o. 2 ^ 

^ y 6s • 



r 






(3.15) 



(3.l6a) 






(3.l6b) 



and 



^ r s 2 n 

0-- ' Vy[ — T - ] 

xy V s 



(3.16c) 



where the states are evaluated at the current state estimates 
x(k). Substituting these expressions in the Q matrix results 
in 






«p3 2 



rp2 



T- 



2 ^xy 



Q = 






symmetric 



T-^ .... 2 .... 

^°xy ^°xy 

xT 2 . 2„..2 T ^ ^..2 

^ 2 ^ ^y 2 ^y 

T2a"2 

y 



0 

0 

0 



T a. 



(3.17) 



The excitation matrix serves not only to take into 
account the possibility of maneuvering, but of model 
inaccuracies as v/ell. Q Is also used to prevent the gains 



of the filter from approaching zero as more and more data 
is processed, by insuring some uncertainty in the predicted 
state values. (See Sage and Melsa [36], page 415 .) 

The observation equations are nonlinear in the states 
and are given by 



z(k) 



0(k) 




f(k) 




- 






y (k) 
x(k) 



+ Vg(k) 






V + VgCos(6g(k)-e(k)) 

^ ' Sj P ^ 



+ V^(k) 



(3.18a) 

(3.18b) 



The doppler equation can be expressed in terms of the state 
variables to give 



f(k) = 



F„(k) Vp 



x(k) v^(k) + y(k) v^(k) 



+ v^(k) 



( 3 . 19 ) 



V. 



P 



(x(k)^ + y(k)^) 



2 s 1/2 



The measurement noises, Vg(k) and v^(k), are assumed to be 
zero-mean and independent with a covariance matrix 



R(k) ■= 





( 3 . 20 ) 



The magnitude of the angle noise is a function of 
the processor used and the slgnal-to-nol se ratio at the 
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sensor. A typical accuracy of ± five degrees is common for 
strong signals and this value was used as the standard 
deviation for most of the simulation runs. 

The resolution of the frequency measurements is 
equal to the Inverse of the record length of the time 
signal. For a 25-second record this would be .04 Hz and 
this value was used throughout the simulation as the frequency 
noise standard deviation. This value would represent a one- 
bin uncertainty in the coefficients of a fast Fourier 
transform processor which typically could compute the trans- 
form in less than 100 ms. 

Equation (3* 4a) can be used to give the linearized 
observation matrix. The result is 



80(k) 86(k) 86(k) 

3x 8v^ 8y 



86(k) 3600 

9^y 3^0 



H(k) 



9f (k ) 8f (k) 8f (k) 

3x 8v^ 8y 



8f (k) 8f (k) 

8v 9P 

y o 



h’hen the derivatives are taken and evaluated at the 



predicted state values x' (k) the result is 



H(k) = 



y* (k) 

x’(k)^+y'(k) 



2 






y'(k)[y'(k)v^(k)-x'(k)v^(k)] | .f'(k)^ x'(k) 



P«(k)Vp 



[x’(k)2+y'(k)2]3/2 



F^(k)Vp [x'(k)^+y'(k)^]^/2 



x’(k) 



x' (k)^H-y' (k)^ 






f»(k) 

F«(k)Vp 



x' (k)[x’ (k)v^(k)-y' (k)v^(k)] 
[x’ (k) 



f'(k)‘ 



P^(k)Vp 



y'(k) 

[x*(k)2-iy’(k)2]^/'2' 



f'(k) 

P!Tk7 

O 



(3.22) 



The $ matrix, Q matrix, R matrix, and H matrix are 
then used in the Kalman filter eauations (3.15). 

In the X-Y filter, both position coordinates will 
change as functions of time. Furthorinore , a change in 
either heading or speed will result In a change ir* both 



n 



/ 




,i 




velocity components. Thus, the calculation of the state 
excitation matrix, Q, involves all state related terms 
except for those involving F^. Since a constant-speed, 
constant-heading target is assumed, a different set of states 
can be found which will decouple the heading and speed 
excitation and involve the estimation of only one linearly, 
time-varying quantity. 

Let be the range to the target at the closest 

point of approach (cpa), X^p^ be the distance to cpa (negative 
before —positive after), v^ be target speed, 6^ be target 
heading and be target rest frequency. The geometry is 
shown in Pig. 3-2. Expressed in this coordinate system the 
state equations are 






= ’'cpa'*^> + ''s-T + S2<^e >^v ■>'> 

o o 



Vg(k+1) = Vg(k) + ) 

s 



(3.23) 



6g(k+l) = ^ 



F^(k+1) = F^(k) + g^(Y^ ) 



with g^ through the random forcing terms. 
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The state transition matrix is therefore 



$ = 



0 

1 



0 



0 

0 . 
0 
1 



0 

0 

0 

0 

1 



The angle observation equation is 



e(k) = 



6s(k) 



1 A-R „,(k) 

tan“^T ^ ] + Vg(k) 



if > 0 

cpa 



8 / “t _ \ 

r, VKy 



tan‘^[ ] -l80 + v^(k) 



X <0 
CD a 



(3.24) 



A is an angular rotation term needed to give the 
correct sign for a given geometry. A is +1 for counter- 
clockwise rotation about the sensor and -1 for clockv.’lse 
rotation. 

* 

The frequency observation equation is 



f(k) = 






V + 

p 






+ v^(k) 



(3.25) 
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The excitation matrix, Q, for the states is found in 
a similar manner to that for the X-Y coordinate system. A 
heading change effects both and while a velocity 

change only effects X . The random excitation will again 

CL 

be modeled as zero mean, piecewise-constant random heading- 
angle velocity, random change in speed, and random change in 
rest frequency. With the expressions for the random forcing 
terms included, the state equations become 

v^(k+l) = V (k) + Y* • T (3-26) 

s ® 

0g(k+l) = BgCk) + Y^ . T 

s 

P^(k+1) = F^Ck) + Y^ • T 



The Q matrix can thus be found and is 



Jii 






I 

p p o I ? ? 

X , R X T ai 

cpa 6 cpa cpa 6 

S I s 

I 



I I s I 



Q(k) = 



(^)^a* ^+R ~a> ^ . -k-R T^ox 0 

2 cpa 0^ { 2 V I cpa 6 i 

s SI s s 



I 1- 



I 



0 



I 



0 



T^a- ^ I 

V ' 

i _L 



Symmetric 



o p 

T-a. 

s 



2 2 
To' • 

" °P 

o 



(3.27) 



Here only the first two rows involve state related terms. 

The nonlinear observation expressions are obtained 
the same way as in the X~Y filter. The resulting matrix is 



H(k) = 



90(k) 


96(k) 


96(k) 


96(k) 


9G(k) 


3R 


9X 




96 


9P 


cpa 


cpa 


s 


s 


o 



3f(k) 


9f(k) 


9f(k) 


3f(k) 


9f(k) 


9R 


9X 


9 V 


96 




cpa 


cpa 


s 


s 


0 



( 3 . 28 ) 



H(k) = 



I 



f ' (k)^.y^(k) -Xlpjk) 

P^(k)Vp R»(k)3 



P;(k)Vp R*(k)^l 





f'(k)2 

P^(k)Vp 



R’(k) 



f»(k) 



(3.29) 



where f * (k) is the predicted frequency measurement and 

R'Oc) = [R;p^(k)2 + x;p^(k)2]l/2 , (3.30) 

is the predicted range. 

The <E>, Q, H, and R matrices are again substituted in 
the Kalmian filter equations at each time point. 

B. SINGLE SENSOR INITIALIZATION 

Por a deterministic problem, since there are five 
unknowns, a set of five independent measurements or equations 
are necessary. V/ith a sensor v/hich measures signal arrival 
angle and signal frequency, this v/oiild require mcasui’emcnts 
at three different times to give six equations . Because of 
the transcendental functions in the doppler expression. 



these equations cannot be so3.ved in closed form to give 
the unknowns, and some type of numerical technique for the 
solution of nonlinear equations must be used. The addition 
of the measurement noise complicates the situation further, 
especially for the angle measurement since the angle noise 
may be large . 

To reduce the effect of noise, the difference In the 
angle measurements at different time points should be as 
large as possible. Appendix C shows the probability of 
having a given error In the angle difference measurements 
as a function of the measured angle difference, with the 
measured difference In standard deviations of the noise. 
Figure Cl shovjs the larger the difference relative to the 
expected value of angle noise, the more accurate will the 
m.easurement solution be. This may be difficult to obtain 
for three time points In a practical situation, since the 
target may not be held for a long enough time to show a 
large angle change due to fading signals or convergence zone 
propagation . 

There is an alternate method whereby an Independent 
measurement can be obtained by looking at the rate of change 
of the doppler shifted frequency with time. The rate of 
change can be approximated with finite difference expressions 
and, because of range dependence In the derivative, the range 
can be calculated with measurements at only two different 
times. Furthermore the heading of the target can be found 
If the sp>eed of the target is assumed knovv'n. The condition 
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Is usually knovm to sufficient accuracy to give a good 
heading estimate. 

The Doppler equation Is 

P 

- f = ^ . (3.31) 

1 + ~ COS(0^-0) 

p 

where f is the doppler shifted frequency, Is the rest 

frequency, v is the target speed, v is the velocity of 
s p 

propagation of the signal, 0 is the target heading, and 

s 

e is the signal angle of arrival . 

Taking the derivative gives 

^ • (-^sl„{ 03 -e).(-f)) , (3.32) 

[1 cos (0 -0)]^ P 

""P ^ 

which reduces to 



dt 



^ = 
dt 



P^ v^ 
o p 



v^ sln(0 -0) 

o o 



^9 

dt 



(3.33) 



The transverse velocity about the sensor is 

with the target range, which when substituted into the 
previous equation gives 
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dt 



(3.35) 



f . p 



Solving for the range yields 



F V 

R = - . 4^: . 



o 



(de/dt)^- 



(3.36) 



Substituting finite differences for the derivatives gives 



Ro “ - 



P -v^'Af-At 
o P 

f2(A9)2 



(3.37) 



Since P^/f » 1 this gives 



R 



o 



v_ • Af • At 



f . (A0)^ 



(3.38) 



Equation (3.33) can be solved for the heading by writing 



Vg sln(eg-e) = 



Po • Vp . (df/dt ) 

f^(d0/dt) 



(3.39) 



and by substituting finite differences for the derivatives, 
and assuming some value for v . the result is 

o 



sin(Og-0) 



V -Af 

P 

1‘ • a 0 • V 

s 



53 



(3.'^0) 



There are two solutions to this equation since 
sln(0) = sln(l8O-0). These two solutions correspond to 



the up and dovm doppler case, that Is 



0 = 180 + 0 - sln“^[- 

s Ur 




(3-^la) 



and 



®s DOWN 



0 + sin ^[~ 



V -Af 

_£ 



(3.^1b) 



f • A0 • V 

O 



The exact difference equation solutions are presented 
in Appendix A. The derivatives above have been approximated 
at the angle midway between the two data points. Figure 3*3 
shows the geometry for the calculation. Note that the range 
can be found independently of velocity. These four quantities, 
the target range and target heading, the target bearing and 
assumed velocity are sufficient to initialize the extended 
Kalman filters because all of the initial states can be 
calculated from them. 

The first quantity to be calculated is the initial target 
heading 6gj> from the equation (3-^la.) or (3-*!lb). The value 
of the angle difference, A0, comes from the two angle observa- 
tions which satisfy the criterion of being greater than three 
standard deviations of the measurement noise apart. With 
this condition and assuming normally distributed laeasurement 
errors. Appendix C shows that the probability that the 
fractional error in tlie angle measurement, 0^/A0, where 0^ 
is the meas\jrement error, and A0 is the measui'^ed angle 




I A ^ [ > 3 C7^ 

^s, 180 -sin-' [- 




F 

01 



F 

AVE 



Vp Af At 
(A0)= f 

4 

[l + ^COS (0g| 



"'si 




] 



Fig. 3.3 Calculation of Initial Range, Heading, and 
Rest Frequency 
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difference, is less than .5 is equal to .7. This criterion 
was somewhat arbitrary but was found to give satisfactory 
results in most instances . It represents a compromise 
between good initial values and long waiting times to start 
the filter. Note that instead of using only the endpoint 
measurements to calculate the midpoint angle, any other 
measurements of the angle taken in that Interval may be used. 
The appropriate weighted average is used to obtain the 
result. For periodic measurements this is just the simple 
average of the m.easurements . The midpoint angle calculated 
in this manner is referred to as 

The value of the frequency difference, Af, is obtained 
by subtracting the first and last frequency measurements 
obtained when the angle condition is satisfied. The quantity 
V comes from the known propagation velocity, f is taken as 
one of the measured frequencies, and v^^ is the initial 
guess at the target speed. When the quantity 



sin(0 



sl"®AVE^ 



Vp-Af 

A0 .f -V 



si 



(3.42) 



is calculated and its absolute value is greater than one, a 
correction on the velocity guess may be made to bring 
|sin(0gj-0^yg) I less than one. Geometrically this means 
that for the quantities measured, the speed of the target 
v;as not large enough to traverse the angle measured. 
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V7hen this is done may be computed using equation 
(S.^la) or (S.^lb). The approximate knowledge of the rest 
frequency and the frequency measurements are usually suffi- 
cient to determine whether the target Is up or down doppler. 
The range to the target can then be calculated by using 
equation (3-38) . 

The Initial rest frequency can be calculated by averaging 
the doppler equations, again over the time that A0 vjas being 
measured. The doppler equation Is 



= 



Ol 



1 + ^ oosce^j-ep 



(3-^3) 



Vi 

where f^^ and 6^^^ refer to the 1 measurement in the start-iip 
interval. F^j Is the initial rest frequency to be calculated. 
Solving for the initial rest frequency gives 



J'ol = ^ cos( 63 j-ep] 



(3.^^) 



and averaging over the initial KJ measurements gives 



KJ 



V 



si 



ol 



KJ ^1^^ Vp cos(6gj-8j_)] 



(3.^5) 



which is, for small angle differences betv;een angle measure- 
ments, approximately 



ol 



v_. 

p r 1 + — ^ 

AVE*--^ V 



p 



cos(0gj 



.)] 



(3.^t6) 
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Once the Initial range, heading and rest frequency are 
calculated, the Initial conditions for each filter can be 
found. For the X-Y filter the equations for the initial 
filter states are 



=‘10 ' ==0 = ’‘o ®AVE 



3Con “ V ^ = V T- cos 0 t 
20 XO sl sl 



^30 =^0=^0 ® 



AVE 



Xiir, “ - y T sin e„T 

40 yo sl si 



^50 ^oI 



(3.47) 



For the R „ filter the Initial states are 

cpa cpa 



10 


^cpal 


A-B^sinCe^I 


20 


X = 

cpai 


V°‘‘(«sl-®AVE^ 


30 " 


V T- 

si 




40 


0 T 

si 




50 " 


P T 

ol 





(3.48) 



v;here A, as before is the rotation parameter. 

The Initial states v/hich are calculated by this technique 
are the estimated state values at a time approximately 
midway betvfeen the two end measurement times and at an angle 
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equal to The state equations are now used to bring 

the states back to the first measurement f^ and 9^ and 
filtering begins with this value. In this manner all of the 
angle and frequency measurements that were not used in the 
initialization phase can now be processed. These would 
correspond to the time points t+T and t+2T in Figure 3.3. 

One result of this initialization technique is to corre- 
late the measurem.ents in the Interval with those states 
which are a .direct function of 0^yg or This violates 

the assumption usually made for Kalman type filters which 
is that all measurement noises are uncorrelated with the 
initial filter states. 

This effect decreases as the number of points, KJ , in 
the initialization Interval Increases. This is due to the 
averaging that vras used to calculate the midpoint angle and 
frequency. Consider the covariance between an angle, 0^, in 
the interval, and the average over that interval, 
which is used for position determination. The covariance is 









^®AVE ®AVEo^^’ 



O-IIS) 



Where 0..., and 0 «,n:. are the mean values. V/riting out the 
lo AVEo 

terms gives 






oy 0j+- • ■ + 0j+0nj °io'''°ao''' ' • •'‘‘‘'lo'' • ' •’'’^KJO 



KJ 



KJ 



)] 



(3-50) 
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which can be rearranged to get 



cov(e^,6^^) = E[(e^-e^^) 




®KJ ®KJO 
KJ 



(3.51) 



Thus, the covariance is 




( 3 . 52 ) 



, 2 

with Og the angle noise variance. The same results apply 



to the frequency measurements and which is used to 

calculate the initial j . 

The other requirement to initialize the filter is the 
initial value of the covariance matrix. Since all of the 
initial states for each filter were calculated from the same 
set of measurements, the initial filter states will be 
correlated with each other. This correlation shows up as 
cross terms in the calculated Initial covariance matrix and 
represents the relationship between the initial data and 
the calculation of the initial states. Put another way, 
the cross terms help describe the region in state space 
where the true initial states are most likely to be as 
specified by the given measurements and the method of 
initial state calculation. 
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C. MULTIPLE SENSORS - FREQUENCY ONLY 
1. X-Y Filter 

To add additional sensors to the filter, the 
observation equations at the new sensor must be found. 

Thus, in equations (3.l8a) and (3.l8b), the predicted x and 
y distances to the new sensor were used in place of the 
predicted states x and y. If the new sensor is at x^y^, 

D D 

the predicted distances are 

x^ = X' - Xb 

= y' - yg (3.53) 

The differentiation is then performed and evaluated at 
the predicted state values to give additional rows to the 
H matrix. 

With an additional sensor of frequency only a 
different initialization scheme was used. The rest frequency 
and target heading, midpoint angle and approximate range, 
were obtained using the single sensor equation. From the 
doppler equation for the second sensor an average angle 
from the sensor to the target can be approximated. 

Let be the unknown angles to the target from the 
second sensor, measured during the initial measurements on 
the first sensor. Thus 1 goes from 1 to KJ . The frequencies 
fj^ are the observed quantities. Thus 
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( 3 . 54 ) 



f.. = 



ol p 



''p ''sl'=°s<9sI-®P 



KJ frequency measurements will be made from the 
second sensor and It will be assumed that they are made at 
the same time as the measurements on the first sensor. 

Adding the above KJ equations and solving for the 
cosine cosine terms gives 



KJ 

E cos ( ) 

i=l ^ 



V. 



V 



si 



KJ 

T. 

1 = 1 




Let 

cos(6si-e2AVE^ 




cosCe^j-e^^) 



(3.55) 



and 



KJ 

^2AVE KJ q 



Therefore , 



®2AVE ®sl ± cos 



(^ 



si 



ol 

2AVE 



- 1 )] 



(3.56) 



As Illustrated in Fig. 3-4, there are two solutions 
for the angle from the sensor to the target. Each of these 
can be used with the avoi’age angle over the same time 
Interval measured from the first sensor. The solution to 
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®2 AVE " ®sl 




R.. = 



d-csc(<!)- 



c tn ( ({) - j - Ci,n(<f )-62 




Fig. 3.4 Initial Range vn’th Second Frequency-Only Sensor 
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this trlangulatlon problem which is closest to the single 
sensor range is then used as the initial range for the 
filter. 

The reason the second sensor is used at all in the 
initialization is to insure that the measurements of 
frequency of the second sensor v;lll be reasonably close to 
the predicted values that the filter will obtain for its 
first few samples. If this is not done, the predicted 
values may be so far off that the linear expansion of the 
frequency term for the second sensor used for the H matrix 
is invalid, and the residual will be so large that divergence 
occurs . 



2. R Filter 

cpa cpa 

With the R „ filter it is more complicated to 

cpa cpa ^ 

add additional sensors because the states were functions of 

the first sensor only and not of the coordinate system as 

is the case for the X-Y filter. The derivatives in the 

observation linearization must be taken with respect to the 

states of the first buoy. Equations (3-2^) and (3*25) for 

the observed frequency and bearing angle at the sensor are 

general equations if R „ and X are referenced to that 
^ ^ cpa cpa 

sensor. As shown in Figure 3.5> R r^c« ^ 

second sensor can be written in terms of the R „ and X 

cpa cpa 

of the first sensor, which are states of the filter. In 
these expressions the angular rotation parameter A, the 
distance to the second sensor, d, the bearing angle to the 



6^1 







Fig. 3.5 Geometry of 2nd Sensor for R —X 

cpa cpa 



Filter. 
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second sensor (p, and the target heading, 0 , are all 

5 

significant factors. The result is 



R = R + A-d*sin(4)-0 ) 
cpa2 cpa ^ s 



cpa. 



= X 



cpa 



d cos(4>-0 ) 



( 3 . 57 ) 



Note that this calculation could result in a 

negative R ; however, this indicates that the angular 
cp 3. 2 

rotation about the second sensor is different from the 
first sensor. 

The resulting observation equation for the second 
sensor is 



f2(k) 






Vs(k)-(X^pa - d.cos(<i)-0g(k))) 



( 3 . 58 ) 



V + 

P 



B^Ck) 



where , 



R 2 (k)^ = (Rcpa(k) + A • d • sin((f)-9g (k) ) ) ^ 

+ (X^^^(k) - d.cos((|)-0^(k)))2 , (3.59) 



Also, 



©2(k) = 6g(k) - arctan ( 



A* (R (k)+A •d*sin((()-0g(k) ) ) 



^cpa " d-cos((i)-0g(k)) 



) 



( 3 . 60 ) 



66 









( 



There is an additional term of -l80° in the last equation 
if X is less than zero, that is, before cpa. These tv/o 
equations are then differentiated v;ith respect to the filter 
states to give the new rows to the observation matrix H. 

The result, omitting the time argument, is 



9fp 

3^ = ^22 ^23 ^2^1 ^25 ^ 



( 3 . 61 ) 



with 



8f 



2 . ^2 



21 8R 



cpa 



v„R„- 

o p 2 



( 3 . 62 ) 



8f, 



22 9X 



cpa 



^2 

I'o '^p «2^ 



(3.63) 



8f, 



23 8v 



^o-^p ^2 



(3.611) 



8f, 

2ll ^ 8T 



^2 ^s ^ 
^o ^p ^2 



[ sln((|>-0 ) 






] 



(3.65) 



( 3 . 66 ) 




and 

3 02 





®23 ®24 



(3.67) 



with 




302 

W~ 



cpa 






022 



302 _ A(R^p^+A.d-sln((f.-0g)) 

^ R 22 “ 



( 3 . 68 ) 



( 3 . 69 ) 



9 6 P 

®23 = ^ 



0 



( 3 . 70 ) 



>24 



30, 



= 1 + 



d(X^^^.cos(^- 03 )-A.Rcp^.sln(c^- 0 s)-d) 



( 3 . 71 ) 





( 3 . 72 ) 
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IV. DIRECT METHOD OP COVARIANCE CALCULATION 



In Appendix B a method of calculating variances for 
functions of Independent random variables was extended to 
Include the calculation of covariances as v/ell . This 
technique was used in order to calculate the initial 
covariance matrix for the two types of filters . The 
resulting values were in some cases an order of magnitude 
larger than one would expect based on the accuracies of 
the measurements and the geometry of the problem. 

The reason for this is the highly nonlinear nature of 
the functions involved In the Initial condition calculation. 
The computation of the heading angle, for example, consists 
of the arcsine function whose derivative approaches infinity 
as the argument approaches plus or minus one. In reality the 
range of values for the arcsine function is bounded by plus 
or minus ninety degrees which represents the target being 
at cpa. Thus when one uses the derivative formulation the 
variance or approximate error appears much larger than It 
really Is . 



A. INITIAL COVARIANCE MATRIX 

An alternate approach was developed which more accurately 
reflects the error variances of the initial filter conditions . 
The measurements Af, A0, sind the Initial speed guess 

are considered to be Gaussian random variables with known 



69 



standard deviations based on the measurement noise and the 
assumed speed accuracy. The changes that occur in the 
calculated initial conditions when each of the measurements 
changes by plus or minus one standard deviation, are 
measures of the extent over which the initial conditions 
will vary and thus are an indication of the accuracy of the 
calculation. These changes can be calculated using the 
boundary conditions Imposed by the geometry of the problem 
and are not restricted to a linear region or a smooth 
function as required in the derivative method. 

In Fig. ^.1 is shown a one-dimensional example of a 
nonlinear function g(x) . The tangent line dravrn at Xj^^ is 
the derivative of the function at x^^* and its extension by 
one standard deviation shows the magnitude of the error 
computed by the derivative m.ethod. Also shown is the direct 
calculation of the changes in g(Xj^) when Xj^* is changed by 
one standard deviation. For this case, the errors one might 
expect are much larger than the derivative calculation 
indicates. The difference of this up and down direct 
calculation, divided by two, is therefore taken as an 
equivalent "standard deviation" for the calculated value. 

This is done for each of the three initial measurements and 
^sl* ^hlle no Implication is made that this value represents 
the true standard deviation of the particular initial 
condition, it does represent, for monotonlc functions, the 
range of values for a one standard deviation cliange in the 
measurements. This in turn is a measure of the accuracy of 
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Fig. 4.1 Taylor Series Ejcpansion Versus Direct Calculation 
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of the calculation and Indeed this is what is ultimately 
needed for the initial covariance matrix. The off-diagonal 
terms in this matrix can also be calculated using the method 
of Appendix B with the derivative times the measurement 
variances replaced by the calculated "standard deviations" 
of the initial conditions in equation (Bt 20). The sign of 
the covariance terms will be preserved by considering only 
positive changes in the measurements. An example for the 
R -X filter illustrates the procedure. Prom equation 

Cp3. Cpo. 

(3-^8) the initial range to cpa from the sensor is 



R „T = A*R^‘Sin(0 , 

cpal o 'si AVE ’ 



(i^.l) 



which from equations ( 3 - 38 ) and (3.^2) gives 



R _ = A • (— jr 

cpal A6^-f 



Af -T- V Af «v 

■ £) . ( P_) . 



A6-f .V 



si 



(^. 2 ) 



Rearranging terms yields 



T« V 



^cpal ^ 



si 



A6 



(- 



Af >v 



A6 «f *v 



L)2 



sl 



(il.3) 



which can be expressed as 



R T ^ A 
cpal 



T • V 
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Written in this form the boundary condition on the value of 



H T 

cpal 
one . 



is evident since the sine term is never greater than 



The equivalent standard deviation in due to a one- 

standard deviation change in A0j for example, is 



^Rcpal^^®^ 2*^^cpaI^^®‘^^A0^ " ^cpal ^ ^ 



i^.5) 



In this case the boundary condition is im.posed in the 

calculation of the sine term in equation (4.4) above and 

T* Vsl 

the maximum value for the range is — . Thus 



|sin(6si ®AVE^ I 



Af^ 



f -V 



< 1 



(4.6) 



si 



This calculation is done for each of the measurements and 
for Vgj. The total equivalent "variance", since the 
measurements are independent, is thus 



Ur> ^ = Op ^(A9) + Op ^(Af) + Op ' ^{v j) (4.7) 
cpal ^cpal ^cpal ^cpal 



Similarly, the equation for equation (3.38), the 

distance to or from cpa can be used to calculate its 
equivalent "standard deviation". Since 



X T = R^‘Cos(0 T--0.^™) 
cpal o si AVE 



(4.8) 
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can be substituted from equation ( 3 * 38 ) to give 



T* V 



si 



cpal A0 



sinCBsi-eAYE) ’ (®sl"®AVE^ ’ ^^-9) 



Combining the trigonometric terms yields 



T*v 



si 



sin 2(0gj-eAYg) 



'cpal 



A0 



(^. 10 ) 



In this instance the geometric boundary is imposed on the 

calculation of the angle 0 t ~ ©A-trc ^nd restricts X to be 

° si AvE cpal 

T . Vsl 

no greater than the maximum range allowed which is — . 

A u 

The equivalent "standard deviation" can be. calculated 
as was done for s^nd the equivalent covariance found by 

an extension of equation (BI 7 ). This results in 






0p (Af)*0y (Af) + Op Y • 

cpal ^cpal ^cpal \pal 



(^^. 11 ) 



The entire initial covariance of estimation error matrix 
can be filled out in this manner. 

To demonstrate the differences in the two error 
estimates. Table II contains the initial covariance matrix 
for a computer simulation in which the target v;as near cpa 
when the filter was initialized. These values were calculated 
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using the partial derivative expansion described in Appendix 



B. The run was made using the R -X filter. In the 

cpa cpa 

upper left is the variance of j • Next on the right is 

the covariance of ^cpal’ covariance of 

R T- and v etc. 

cpal si’ 

The diagonal terms, which represent the initial error 
variances, can be expressed In more convenient units to 
give 






cpal 



^ = (li}.7 K yds)^ 



or 



'R 



cpal 



= li|.7 k yds. 



Similarly 



and 



o 



X 



cpal 



a. 



V 



si 



'si 



'p 



ol 



14.4 K yds , 
1.68 yds/sec , 
71.7° 

2.5 Hz. 



(4.12) 



The accuracy of the initial estimates is typically much 
better than these standard deviations indicate. With the 
same set of data, the direct calculation described previously 
was used to calculate the initial covariance of error matrix 
and the result is presented in Table III. The standard 
deviations for the diagonal terms are nov/ 



216.96 


184,20 


- 5.73 


” 17.07 


33.80 


l 8 i .20 


207.28 


-16.43 


-17.83 


36.37 


-5.73 


-16.43 


2.85 


1.21 


-2.63 


-17.07 


-17.83 


1.21 


1.56 


-3.16 


33.80 


36.37 


- 2.64 


-3.16 


6.16 



TABLE II. Initial covariance matrix using partial 
derivative expansion. 



78.84 


30.34 


-0.23 


-3.89 


7.20 


30.34 


39.29 


- 7.84 


-3.52 


7.36 


-0.23 


- 7.84 


2.85 


0.60 


-1.39 


-3.89 


-3.52 


0.60 


0.34 


-0.69 


7.20 


7.36 


-1.39 


- 0.69 


1.43 



TABLE III. Initial covariance matrix using direct 
calculation . 
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= 8.78 K yds 



a 



a 



a 



a 



R 



cpal 



X 



cpal 



= 6.27 K yds 



si 



= 1.68 yds/sec 



'si 



= 2*5 RO 

J ^ • J y 



y 



y 



y 



and = 1.2 Hz . (^.13) 

ol 

The net effect of the partial derivative method of covariance 
matrix calculation is to deemphasize the initial conditions 
in favor of the first several measurGinents . The second 
method applies a filtering weight more in line v;ith the 
accuracy of each of the initial states. 

Not only are the magnitudes of the errors quite different 
but the orientation of the error ellipses which are determined 
by the covariance terms are different as well. This is 
illustrated in Pig. ^.2 for the position variables 
Xcpai • the two covariance matrices in Tables II and III 

a coordinate rotation is applied to give uncorrelated position 
coordinates. Figure ^.2a shows the orientation to be aligned 
with the computed values of 3^hd ^„pgjj not dependent 

on the sensor position. Figure ^.2bj for the direct calcula.- 
tion, shov/s the error ellipse oriented v/ith the bearing line 
from the sensor to the target at the initial positloji. This 
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Fig. 4.2 Error Ellipses for Initial Covariance 
Calculation 
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rotation is believed to occur because of the boundary 
conditions imposed on the calculated errors and represents 
a more precise picture of the uncertainty regions associated 
with the initial state values. 

B. GENERAL COVARIANCE UPDATE AND APPLICATION 

TO KALMAN FILTER EQUATIONS 

One of the problems with Extended Kalman filtering is 
that the linearity assumption used when the nonlinear terms 
are expanded m.ay not be valid, particularly in the early 
stages of filtering when the initial filter states are 
significantly different from the true values. There may 
be cases where boundary conditions impose constraints on the 
range of the nonlinear functions which cannot be handled 
easily using the Extended Kalman approach. In the final 
analysis the linear expansion will only be valid when the 
error estimates accurately approximate the true errors in 
the problem. 

To obtain these accurate error estimates, the method 
developed for the calculation of the initial covariance of 
estimation error matrix described previously, can be applied. 
In this development one addltioanl point becomes clear: 
the partial derivative method (Extended Kalman), requires 
the linearity condition to be valid for a region in state 
space extending one standard deviation in all dimensions. 

When this condition is not satisfied, the direct change 
method v/ill provide more accurate approximations to the 
errors . 
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Consider a discrete problem with n states and m 



observation variables. 

x(k+l) = f(x(k)) + w(k) 

^(k) = h(x(k)) + v(k) (^.1^) 

For an Extended Kalman filter the equations have the form 
x(k+l) = $(k) x(^) w(k) 

z(k) = H(k) x(k) + v(k) , (^.15) 



with 



^(k) 



3f - 

As. 



8x 



x=x(k) 



(^!. 16 ) 



and 



H(k) 



3h 



9x 



x=x* (k) 



(^.17) 



The predicted state covariance matrix is found from 

P'(k) = <I>(k) P(k) ^Ck)"^ + Q(k) (4.l8) 

v/here P(k) is the current covariance of estimation error 
matrix. To see how the linear expansion affects this 
calculation, pre-factor and post-factor a diagonal matrix 
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of the standard deviations of the states out of the 
covariance matrix. P(k) Is given by 



P(k) 




COv(Xn x^) 
1 ^ 



cov(x^X2) 







covCx^Xj.^) 




Performing the pre- and post-factoring yields 

r~ 



P(k) = 



"1 



x^ 



0 






cov(x^X2) 

^x 

Xi X2 



cov(x^X2) 
a a 

Xi X2 



cov(x^Xj^) 
CJv (Jv 



cov(x^x^) 

V 
1 n 



X, 



X, 



"xn 



(^1 .20) 



or P(k) = I (k) R(k) E (k). 



(^i.21) 
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Z is the diagonal matrix of standard deviations and 
R is recognized as the matrix of correlation coefficients 
whose elements are 



If the 
result 

P’(k) = $(k).Z(k)'R(k)*z'^(k) + Q(k) (4.23) 



cov(x . X . ) ■ 

[R].,. = — — 

^x ^x 
^i 



(4.22) 



prediction equation (4.l8) is now applied, the 
is 



The matrix product $(k)*Z(k) is given by 



$(k)E(k) = 



r 91^^ 




9fi" 
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9x^ 


9f2 


■ 
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• 

• 
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9X2 


• 

• 

• • 
• 




^2 

» 
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9f 

n 

9x^ 
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V 



8f. 



3Xi *^x^ 



3f 
n 



3fl 

Sx^ *^X2 



9f2 

9X2 *^X2 



• • • • 



^^n ^n 



9f 



n 



9x^ -n 



(4.24) 
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The elements of this matrix product, represent the linear 
expansion of the function f(x(k)), carried over a one-standard' 
deviation interval of each of the states. Thus if the linear 
expansion is not valid over that interval, inaccurate esti- 
mates of the predicted covariance could result. 

However, if each element in the $(k)Z(k) matrix. 



then, even though the function is nonlinear in the states, 
the values obtained for use in the prediction equation (^, 23 ), 
will be a better estimate of the actual errors. 

Each of the Extended Kalman filter equations (3*5a) 
through (3.5f) can be written using the direct change 
calculation. Let F(k) be the direct one-standard-deviation 
expansion matrix for the nonlinear state vector function 
f(^(k)), and H(k) be the direct expansion matrix for h(x(k)). 
Then the Extended Kalman filter equations are 



3f^(x) 



CT 



is replaced by 



8x . 

J 




2 



(^. 25 ) 



P’(k) = P(k-l)R(k-l)F (k-1) + Q(k-l) 



= Z* (k) R’ (k) Z’ (k) 



(i<.26) 
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{^. 21 ) 



G(k) = Z’(k)R» (k)H»'^(.k)[H(k)R' (k)H'^(k) + R(k)] ^ 

P(k) = P’(k) - Z’ (k)R’ (k)H'^(k) •CH(k)R» (k)H'^(k)+R(k)]"^- 

•H(k)R' (k)Z* (k) 

= [Z'(k) - G(k)H(k)]R' (k)Z» (k) . (4.28) 

Z'(k) is the predicted pre-factored standard deviation 

matrix and R’(k) is the predicted correlation coefficient 

/\ 

matrix. The filter equation for x(k) and the prediction 
equation for x'(k) remain the same. 

Any geometric or physical state boundaries can be applied 
directly to the calculation of equivalent “standard deviation" , 
given by equation (4.25) j to reduce the range of the function 
and thus the uncertainty. 

While the updated values may not carry with them 
probability implications, as indeed they do not in the 
Extended Kalman approach, they would more accurately 
approximate the errors in the updating calculations due to 
the error covariances of the states.. 
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V. COMPUTER SIMULATION 



To evaluate the two types of filters a simulation pro- 
gram was written which computes the true values of sensor 
measurements at each sensor for a specific target track, 
adds Independent Gaussian noise values to these measure- 
ments, and supplies the results for input to the two filters. 

For the extended Kalman .filter in the X-Y coordinate 
system, the provision was made to Include up to two addi- 
tional frequency-only sensors. The Extended Kalman 

R „ - X ^ filter has the p'rovlsion for an additional 
cpa cpa 

angle, frequency sensor, as well as up to two additional 
frequency-only sensors. For the purpose of the simulation 
the rest frequency of the target v/as chosen to be 500 Hz . 

The speed of the target varied between 6 to 12 knots, and 
a wide variety of headings and geometries were run. These 
values were chosen in an attempt to provide realistic tar- 
get parameters to the filters. When using doppler informa- 
tion, the higher the frequency, the more the doppler shift 
and the better will be the freqiiency measurement for a 
given resolution. At 500 Hz, a difference in speed of one 
knot, provides a shift in frequency of approximately .17 
Hz. 

A. MONTE CARLO SIMULATION 

In order to compare the performance of the two filters, 
and of each individua], flltei*'as various parameters W '-'-‘e 
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changed, Monte Carlo simulation runs- were made. The calcu- 
lated statistics of these simulation runs then were used to 
contrast filter performance. Each run used input measure- 
ments (chosen for convenience to be equally spaced in time) 
from the simulation program. After each Monte Carlo run, 
the statistics of the filter states at each time point were 
calculated. The sample means, x, y, sample variances, 
var(x), var(y), and sample covariances cov(x,y) for the 
position variables were com.puted. Let the position outputs 

of the filter at time t(k) for a particular run be x^(k) 

1 

and y^(k). The sam.ple statistics for N runs are 






N 



x(k) = Z X. (k) 
1=1 ^ 



(5.1) 



1 N . 

y(k) = Z^y^(k) 



(5.2) 



var[x(k)] = I I x^(k)^ - Nx(k)^] (5-3) 

N 

var[y(k)] = a ^ = r~ [ Z x. (k)^ - Ny(k)^] (5.^) 

y IN X i^l X 






N 



/N /N 



cov[x(k)y (k)] = r^yO^Oy = [ Z Xj^(k)y^(k)-Nx(k)y(k) ] 



(5.5) 



^Deutsch [l6] gives the equation for the sample variance 
which can be readily extended for the sample covariance. 
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The choice of how many runs to use in a Monte Carlo 
simulation is always a difficult one and must be a compro- 
mise between the computer time for the simulation program 
and the accuracy of the computed statistics. All the results 
used for filter comparison in this work represent 200 Monte 
Carlo runs, Bucy [11] provides a means for estimating the 
accuracy of the statistics calculated from Monte Carlo runs 
and his findings are summarized in Appendix D and the 
results given below for the x variable. 

For 200 runs the probability that the magnitude of the 
difference between the specified mean, y, and the sample 
mean will be less than ten percent of one standard deviation 
is .84. That is 

Pr {|x - y] < .1 var(x) } = .84 . (5.6) 

The probability that the specified standard deviation 
will be within .1 of the sample standard deviation is 
approximately .95. That is 

Pr {.90^ < a < l.la^} ~ .95 (5.7) 

— X .A. 

with o the specified standard deviation. 

B. ROTATED ERROR COORDINATE SYSTEM 

The position variances and covariances calculated by 
the above technique indicate.^ that the errors in the X and 
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Y position are highly correlated. Correlation coefficients 
of . 8 or .9 are typical at each time point. 

If the errors were normally distributed this would 
indicate that there exists a rotated coordinate system 
such that in the new system the orthogonal position com- 
ponents are uncorrelated. This is equivalent to taking the 
exponent of the joint noi’mal probability density function, 
and applying a coordinate transformation which eliminates 
the cross terms. 

The exponent (for zero-mean random variables) is 




2r xy 
xy + 

a a 
X y 




'h 



:: ^ 



(!5.8) 



When set equal to a constant, this curve, which is an 
ellipse, is a curve of constant probability. This ellipse 
does not have its major and minor axes aligned vrith the 
coordinate system however. By applying the transformation 



x' = X cos 0 + y sin 9 



(5.9a) 



and 



y' = y cos e - X sin 0 



(5.9b) 
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with 



e 





cov(x ,y ) 

2 . 2 

- a 



] 






( 5 . 10 ) 



the ellipse will be aligned with the x’, y' axis and the 
resultant random variables will be uncorrelated. The new 
variances In this system are calculated by 



a 



X* 



2 




cov(xy ) 
sin 20 ’ 



(5.11a) 



and 





cov(xy ) 
sin 20 • 



(5.11b) 



This method of viewing the sample statistics provides insight 

into the filter performance because it shows the regions of 

high probability and their relationships to the sensor, The 
2 2 

values of o , and a , are referred to as the rotated error 
X y 

variances in the follovrlng section. 

C. FILTER PERFORMANCE 

There are almost an infinite number of variables which 
can be changed, and their effect on the filter response 
studied. One of the objectives of this research was to pro- 
duce a workable filter for the passive location and tracking 
problem, and this will be demonstrated through the comparisons 
below . 
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Each figure Is a computer generated Calcomp plot of a 
simulated track. Unless othervi^lse stated, a single sensor 
Is positioned at the origin of the coordinate system. The 
line drawn Is the true track with the tic marks Indicating 
the observation times, and the arrow • Indicating target 
heading. The asterisks are the Monte Carlo filter points 
and are the sample average values of position at each time 
point for the 200 Monte Carlo runs. Some of the figures 
also show the orientation of the final sample position 
error ellipse by showing the one standard deviation major 
and minor axes . 

In all of the simulation runs, the X-Y filter gave 

lower final error variances v;hen compared with the ' 

R - X filter. For this reason most of the simulation 
epa epa 

tests were run with the X-Y filter. 

1 . Time Between Samiples 

This requirement will be dictated ultimately by the 
computational capability of the computer which would be used 
for an actual system. With the resolution used in the slmm- 
latlon set at .0^ Hz, at least a 25-second time record Is 
needed for processing by an FFT processor. The time between 
samples must be short enough to allow the filter to correct 
errors, especially In the Initial phases. If the time Is 
too long the result could be filter divergence or a poor 
estimate of the states. This Is shown in Fig. 5.1 and 5>2. 
The first graph Is run with 200 seconds between samples and 
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Fig. 5.1 Filter Output and Target Track — 
200 Seconds between Observations 
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Fig. 5.2 Filter Output and Target Track — 
100 Seconds Between Observations 
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second with 100 seconds between samples. All subsequent 
simulation runs were made with 100 seconds between samples. 

2 . Effect of Initial Covariance Matrix 

The full Initial covariance matrix, as described In 
Chapter IV, gives as complete a picture as Is possible on 
the probabilistic relationship among the Initial states. 

For this reason it provided better filter transient response, 
and resulted In lower covariance of error values at the end 
of a given run. Fig. 5.3 is the response of the X-Y filter 
when only the diagonal terms of the Initial covariance were 
used - a common practice In implementing filters. Fig. 5.^ 
is the X-Y filter response for the identical set of measure- 
ment data with the full Initial covariance implemented. 

Fig. 5.5 and 5.6, Fig. 5.7 and 5.8 and Fig. 5.9 and 5.10 
show similar behavior. The final rotated error ellipses are 
shovm on the plots. In each case the variances were less 
with the full Matrix than with only diagonal terms. Figures 
5.11 and 5.12 and Fig. 5.13 and 5.14 show similar behavior 

for the ~ ^ filter, 

cpa cpa 

3 . Error Ellipses and Number of Sensors 

Figure 5.15 shows the X-Y filter j^esponse for a 
single sensor at the origin. Figure 5.l6 is a diagram of 
the one sigma regions for this filter when the rotated error 
coordinate technique is used. Note how the rotated system 
tends to align Itself with the bearing angle to the filter 
point. The rotated error ellipses of the velocity components 
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Fig. 5.3 X-Y Filter and Target Track — 

Diagonal-Only Initial Covariance Matrix 
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Fig. 5.4 X-Y Filter and Target Track — 
Full Initial Covariance Matrix 
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Fig. 5.5 X-Y Filter and Targ'at Track - 

Diagonal -Only Initial Covariance Matrix 
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.6 X-Y Filter and Target Track — 
Full Initial Covariance Matrix 
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Fig. 5,7 Filter and Target Track — 

Diagonal -Only Initial Covariance Matrix 
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Fig. 5. -8 X-Y Filter and Target Track - 
Full Initial Covariance Matrix 
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Fig. 5.9 X-Y Filter and Target Track — 

Diagonal -Only Initial Covariance Matrix 
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10 X-Y Filter and Target Track — 
Full Initial Covariance 



101 



V 






i 




Fig. 5.11 Rcpa"^cpa Target Track - 

Diagonal -Only Initial Covariance Matrix 
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Fig. 5.12 *^cpa"^cpa Target Track — 

Full Initial Covariance Matrix 
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Fig. 5.13 Filter and Target Track — 

^ cpa cpa ^ 

Diagonal -Only Initial Covariance Matrix 



10^1 



l \ 



( 




Fig. 5.14 Filter and Target Track — 

Fiill Initial- Covariance Matrix 
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(not shovm) tend to show a similar although not as striking 
alignment with the bearing line . 

In Fig. 5.17 an additional frequency-only sensor was 

added at the position Indicated and as expected, improved 

the averaged filter performance. The rotated variance plot 

is shown in Pig. 5.18 and the presence of the second sensor 

has reduced the variance by a significant amount in the 

radial direction. This result will, of course, depend on 

the sensors and target geometry. For use with this type of 

filter, an additional sensor will enhance performance when 

it sees a definite change in the frequency it is measuring. 

If the sensor were put near the predicted track for example, 

very little change in the doppler shifted frequency would 

take place (except when the target went thru CPA) and the 

changes which the sensor would measure would be mostly due 

to the frequency noise. 

Comparison of the X-Y and R - X Filter 
i c p a c p a 

As mentioned previously the X-Y filter performed 

better in all simulation runs than did the R - X 

cpa cpa 

filter. The Monte Carlo tracks were closer to the true 
track and the final rotated error variances were lower in 
all cases. Figures 5-19 through 5.24 show a comparison of 
the tv7o filters for three particular target tracks. In 
each case the same simulated input measurements v;ere pro- 
vided to the X-Y and R - X „ filters. The same initial 

cpa cpa 

conditions of position, velocity and rest frequency \';ere 
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Fig. 5.15 X-Y Filter and Target Track — 

Single Sensor of Angle and Frequency at (0,0) 
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Fig. 5.16 Plot of Major and Minor Axes 
of Rotated Covariance Ellipses 
for Position. One-Sigma Points 
are Marked. Single Sensor 
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Fig. 5.17 X-Y Filter and Target Track — Two Sensors; 

Angle - Frequency (0,0); Frequency (2000, -?000) 
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y Position (Kyds) 



2 Sensors 

Data points are 300 sec apart 




Fig. 5.18 Plot of Major and Minor Axes of Rotated 
Coveriance Ellipses for Position. One 
Sigma Points are Marked. 2 Sensors 
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calculated as starting values for each filter (expressed 

In the appropriate states). 

One possible explanation of the poorer performance 

of the R - X filter lies in the calculation of the 
cpa cpa 

position of the target after an observation has occurred. 

For the X-Y filter, of course, all that is needed are the 

states X and Y. For the R - X „ filter the bearing angle 

cpa cpa 

to the target must be found from 



e(k) 



e^Ck) 



tan ^ ) 



if > 0 (5.12a) 

Cpa 



or 



e(k) = e^(k) 

o 



T A.R (k) 

tan-\-,r-££a ) 



180 If ' 0 , 

(5.12b) 



and the range from 



1 

R(k) = (R,p^(k)8 .f X^p^(k)2)2 . 



( 5 . 13 ) 



The position is calculated from the range and bearing. 
Note that coupling between the position and velocity compon- 
ents has occurred because the heading angle appears in the 
bearing angle calculation. Thus the position calculation 
is dependent on three estimated state values instead of tlie 



111 



J 

1 




Fig. 5.19 X-Y Filter and. Target Track 
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Fig. 5.20 Filter and Target Track 

^ cpa cpa 
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Fig. 5.21 X-Y Filter and Target Track 
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Fig. 5.22 R -X Filter and Target Track 
^ cpa cpa 
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Fig. 5.23 X-Y Filter and Target Track 
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Fig. 5.24 R^pa'^cpa Target Track 
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two required for the X-Y filter. A large noise component 

in the frequency measurement, for example, which results 

in a significant change in the heading, will thus effect 

the position by rotating the R , X , R triangle in 

cpa cpa 

Fig. 3.2. 

5 . Filter Residuals 

Poirot [ 3 ^] in his dissertation studied the residuals 
of an estimation process in order to determine whether biases 
were present in the procedure. For on-line digital flD.tering 
the use of residuals, the difference between the predicted 
measurement and the actual measurement, might be used for 
a similar goal. In the simulation program, a counter vms 
established to indicate when the residuals ■ were greater 
than two standard deviations for the angle and four standard 
deviations for the frequency noise. The frequency is less 
sensitive to position errors. These levels were somev;hat 
arbitrary, but for a poor track on a particular run, the 
counter values were larger when compared to a good track. 

The effect needs further study but Indications are that 
this could be used to reinitialize the filter and reprocess 
the data v/ith the new starting values when the residuals 
show poor performance. 
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VI. CONCLUSIONS 



A. PASSIVE TRACKING AND NONLINEAR FILTERING 

The idea of localization and tracking with a single 
passive sensor has been shovm to be feasible. The method 
of calculating the initial position, heading, and doppler- 
rest“frequency provides the key that enabled the extended 
Kalman filter to perform satisfactorily. With only limited 
a-prlori knowledge, the filter was initialized by waiting 
for a sufficient change in the observations to calculate 
a good initial condition. The state vector was then brought 
back to the first observation time to begin filtering. 

This technique enabled the filter to start v:ith good enough 
initial conditions so that lock-on and track would result. 

The role of the initial covariance matrix was shoxm to 
play an important part in the transient response of the 
filter and resulted in overall better performance as measured 
bj' sample means and sample variances in Monte Carlo simula- 
tions. That is, the average mean-square error at the end 
of a given filter run was less when the full initial co- 
variance matrix v;as used than if only the diagonal terms 
were used. 

The calculation of the covariance of error matrix for 
this nonlinear problem using a simple Taylor Series expan- 
sion was shown to not adequately reflect the actual errors 
that can be expected due to errors in the measured quantities . 
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This Is due to the linear expansion not being valid over 
a variation of one standard deviation In these quantities. 

A method was developed which more accurately estimates the 
errors by a direct calculation of the changes that occur 
In the nonlinear function due to a one -standard-deviation 
change In the input states or measurements. In this way, 
any physical or geometric boundary conditions can be imposed 
directly. In an extension of this technique. It is believed 
that the updating of the covariance matrices used in the 
linear Kalman filter can be accomplished the same way and 
the result will be a more accurate weighting of the measure- 
ments versus the predicted filter values. 

Even with a relatively simple geometric problem, the 
choice of filter states can play an important role in filter 
performance. The choice of the best system will be affected 
by the measurement variables, the coordinate system chosen 
for the dynamic model, and how any filter excitation is 
modeled. 

An additional sensor measuring frequency-only was found 
to give significantly lower position error variances than 
a single angle and frequency sensor. In general it was 
found that the sensor should vlevj as large a change in the 
measurement as possible to minimize the effect of measurement 
noise . 

The residuals of the filter, that is, the difference 
between the actual observation and the predicted observa- 
tion, v;ere found to be a good, qualitative indication of the 
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transient performance of the filter for a particular run. 

The values in the calculated filter covariance of error 
matrix V7ill not show when divergence is occurring. Some 
criteria using the filter residuals relative to the measure- 
ment error can be helpful in this regard. A corrective 
action might be to reinitialize the filter and start 
processing the observations over again. 'The residuals may 
also be used as an indication of a maneuvering target, 
in which case the assumed state model is no longer valid. 

B. SUGGESTIONS FOR FURTHER STUDY 

The most obvious extension of this work would be to 
test the filtering algorithm on real data. The capability 
of obtaining the required frequency resolution has been 
demostrated [ 46 ]. The extreme limits on the angle noise 
have to be defined so that meaningful measures of the 
angle noise can be used in the filter. 

The method of direct calculation of the covariance 
matrices for nonlinear problems represents a different 
approach in applying the linear Kalman filter equations, 
and this technique should be evaluated against the normal 
extended Kalman formulation. 

Further study of the choice of filter states and their 
comparison with each other, is another area of Interest. 

The same pi’oblem considered here could be formulated in an 
R - 0 coordinate system for example. In this system both 
the state and observation equations would be nonlinear 
hov/ever . 
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An additional area of study might include the effects 
of a maneuvering target on filter performance, and how the 
choice of filter states and their coordinate system are 
involved . 



12? 



APPENDIX A 



Developmerxt of Initial Conditions for Single 
Sensor — The Exact Difference Equations 

For the two dimensional constant-speed and constant- 

heading target, there are five unknovms which completely 

determine the solution. Expressed in an X-Y coordinate 

system these are position (x,y), the velocity (v ,v ) and 

X y 

the rest frequency of the emitter, F^. The only a priori 
knowledge available in the general search problem is the 
approximate target speed and an approximate value for . 

The measurements consist of a noisy angle of arrival and 
a noisy doppler-shlfted frequency. Using the change in 
angle and the change in frequency, an expression for target 
range and target heading can be found which is not sensitive 
to knowledge of the rest frequency. The accuracy of the 
range and heading calculation can be controlled by waiting 
until the changes in angle and frequency are large with 
respect to the standard deviations of the noises involved. 

An expression was derived from recognizing that the rate of 
change of the doppler-shlfted frequency is range dependent. 
Given range and speed, the heading is then found from the 
angular velocity expression. 

From Fig. A-1, at time t, an angle 6^, and frequency f^ 
are measured and at time tPAT, &2 and f^ are measured. The 
doppler equations are 
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Fig. A-1 Geometry of Sensor 
and Target. 
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f = p 



1 + ^ oos(e^-ej^) 



and 



(A-l) 



1 + ^ oos(e -e ) 

P 



(A- 2 ) 



where P^ is the rest frequency and v is the velocity of 
propagation of the signal. Time differences in the arrival 
of the signal have been neglected since they are small 
compared with the time scale of changes in the frequency 
and angle. Subtracting A-l from A -2 
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''p ■ (P„/fp ■ (p^/rp 
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A0 = 02“®1 
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where 0^, i = 1-KJ, are the angle measurements taken at 
equal time increments betv/een t and t+AT. Refer also to 

Pig. 3.3. 

Then 



^s‘^l’^2 Afl 



Solving for sin (0^-0^^) yields 






2 sin ^ •fi-f 2 'V 3 



(A-3) 



There are tv7o solutions to this equation 



®AVE ^ 



Af.p -V 
o P 



o • A0 „ „ 

2 sin .f-i *fo*v^ 
2 12s 



] , 



and 



18o»-(63-e^^) = sln-l [ - 



Af.p .17 

O P 



2 sin ^ Ti-fs-Vg 



] . 



The first solution is the dov/n-doppler or receding 
target and the second solution is the up-doppier or 
approaching target. Thus 



®sDWN ■■ ®AVE f " 






(A-l|) 



2 sin -■ 



AO 



q-''2'''s 



and 
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®sUP ®AVE. ^ 



Af*P -v 
o P 



2 sin 



A0 



(A- 5 ) 
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The inverse sine is taken in the interval [-^s • From 

the approximate knov;ledge of F^ the up or dov/n-doppler case 

• o 

can usually be determined. For small angles and since 
F 

7 — ^ ~ -sr- the expressions simplify to 
^ 1*^2 ^2 



®sDWN ^ ®AVE ^ 

and 



- 1 , 



Af -V 



P 



®sUP ’ ®AVE ^ AG-fo-v, 



(A- 6 ) 



(A- 7 ) 



Using the Law of Sines on the tv;o triangles in Fig. A -1 gives 

a ^ R 

sin 4 ^ sln(0„-05) 

d O C- 

and 

b ^ R 

sin ~ sln(0^-02) 

Now a+b is the distance traveled in time AT and thus 
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a+b = v^-AT = R sin — • [ 

S d. 



slnCBg - e ^) sin ( eg - e ^) 



= R ,1n M . ^ sln(e^-9p 

2 sln(e -6-) • sinCO -6^ ) 

■ o d_ 



A0 



= R sin 



A0 ^ 2 



^ |[cos A8 - cos 2(0g-0^^)] 



Solving for R and substituting for cos 2(0 -0..^) = 

S A VHj 

1-2 sin^(0g“0^yj,) , ■ and sin ~ cos ^ ~ \ sin A0, the 



result is 



„ _ v^.AT ^ 2 sin ( 03 - 0 ave) + cos A0 - 1 
K - —it:: — r-r L o ' U "r 7 7; ?T ^ J 



sin A0 



2 sin ( 0 g - 0 ^^^) 



v ^- iT - slnO ^- e ^ i ^;) 



sin A0 



[1 + 



cos A0 



2 sin (0g 0/^Yg) 



Substituting for sin(0g-0^yg) from equation (A-3) 
Af • V *F • AT 

R = - T“ [1 + (cos A0-1)* 2* sin 

2 sin *sin A 0 *f 2 ^-f 2 



V f f 

2 AO. ^s 1 2 s 2 -, 



Af . Vp . P^.AT 



2 sin ^ -sin A 0 *f 2 ^-f 2 



2 '^ s ’^ l’^’p ^ 

-[1 - (1-cos 

o ' p 
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the 



For typical values of A0, Af, v„ , v„, f , , and F , 
magnitude of the second term is small compared to one and 
can be neglected. For example with 



A0 = 15° 

Af = .2 Hz 

‘^2 

^= 500 Hz 
o 

= 5.63 yds/sec 

o 



f-, -f. 



(1 - cos A0)2(-|— 



A f • P • v„ 



P 



.0837 



Now using the approximations used previously for the 
heading calculation and neglecting the term above 



Af-v -AT 
P 

(A0)^ 



The error variance of the Range and heading calculated 
above can be detei'^mlned from the equations in Appendix B 
and a knov;ledge of the variance of Af,A0, and v . Let 

S 



var(Af) = 
var(A0) = 20q^ 

p 

var(v^) = 
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i 



Then from equation (B-1) 



var (R) 



^2 2 , /^n 2 _ 2 

^3A6^ ^3Af^ 



R^[ 8 -^ + 2 -^ ] 



A6 



AT 



var (6s) == 



.2 2 . ,^0s.2„ 2 . 2 , ,'®s.2_ 2 



+ ^^a + (taT^) CTa/ + 



AVE \AVE ^s 



This is shown in Appendix B to be 



2 ^ 2 2 2 2 

"e , ”cpai r, , -. “e . °ys -, 

la ■ 1 2 772 * ^ 7 : 2 * 



^cpal 



Af 



A6' 



For 



Ae = 15 ° 

Af = .2 Hz 



- (5°)^ 



= (.02 Hz)^ 



V 3 = 5.63 yds/sec cr^ ^ (^^gg yds/sec)^ 



var(R) = R^(| + .02) ~ . 9R^ = (3600 yd)^ 



R = 3830 yds 



var(e ) - .0038 + .2^16[.02 + .22 + .O'l] 



= ( 15 °)' 
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APPENDIX B 



Covariance Expressions for Single Buoy 

Initial Conditions — R Filter 

cpa cpa 



The initial conditions for the filter are calculated 

from the three measurements described in the text: Af, the 

frequency difference; A0, the angle difference; and v j, the 

assumed target velocity. It is assumed that Af and A 0 are 

independent, normally distributed random variables whose 

means are unknovm and whose variances are obtained from 

the measurement accuracy. The variance of v^j is based on 

the a priori knowledge of target speed and it will be 

assumed to be (3 knots) . If each frequency measurement has 
2 

a variance , then the difference of two Independent 

2 

measurements has variance . Likevrlse the variance of 

2 

the angle difference measurement is 2a ^ . The uncertainty 
due to the frequency measurement Itself will be neglected 
since it is small compared to other errors, (a^ /f /A0 ). 

The angle is the mean of the angle measurements taken 

before the filter is initialized 




(B-1) 



such that 



O(tj^j) - 0(1) > 3-ag 



(D-2) 
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The initial filter conditions are derived from the 
following expressions given in Appendix A. 



A-Af^.AT-v 
R - = 2_ 



cpal .-3 „ 

^ Zi0-’ • V _ • f 

si 



(B-3) 



Af • AT • V, 



^cpal 



P)2 



- ( 



Af‘=-AT-v 



he -T. 



Ae3-v^l-f2 



P_)2^1/2 



(B-4) 



V 



si 



= V 



si 



(B-5) 



si 



®AVE “ ^ 



Af • V 



) - 0 



^®-^sI-f‘2 + 180° 



(B-6) 



^oI ^AVe'-^ Vp cos(6gj ^AVe'^^ • 



(B-7) 



The initial covariance matrix consists of all second 
order expectations over the probability space of the four 
random quantities discussed earlier. The effect of other 
unknowns (Vp, f 2 ) are assumed small compared with the above. 

Papoulls [ 32 ] gives an approximate expression for the 
mean and variance of a smooth function of two random 
variables. Below an extension is made for n-random variables 
and an expression for the covariance between two functions of 
these variables is found. Since Af, A0, and v^ are 
considered Independent random variables, and thus thejr 
covariances are zero, the expressions can be simplified. 

Also since is tBe sum of independent random varlabDos 
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i 



and A9 is the difference of two of them, 
are independent. 

Let X be a vector of independent random variables and 
g(x) and h(x) be two scalar valued functions of the vector 
X. It is assumed that g(x) and h(x) are sufficiently smooth 
functions in the region of the mean of x, that a Taylor 

expansion of g(x) and h(x) about ^ can be made. It is also 
assumed that the probability density, functions for x is 
symmetrical about the mean and sufficiently compact so that 
central moments higher than the fourth do not need to be 
considered. If g(x) is expanded about its mean, then 



g(x) = g(3£o) + If- (x-Xq) ->• I + ••• 



1 0 



(B-.8) 



^ O' , 

vjith derivatives evaluated at x = x„ . The vector — — is 



— — o 



defined by 



3x 



(1£)T k I Ih. 



3X 



3x, ' 3x, 



' ’ 3X 



n 



(B-9) 



3 p* 

the matrix [^- " -] is given by 






iffi. 

3x = 



2 

9 g 



9g 



3 X ^9 ^2 



3X2c)x^ 



d r, g 

3x^3Xj_ 



3g 



3x- 



'^2 

3 









(B-10) 



9x. 



n 
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Taking the expectation of the above expression and 
remembering that the density of x Is symmetrical and that 
covariances of the form E[(x.-x. )(x.-x. )] = 0 because of 
the Independence condition, the following result is obtained. 

2 

E(g(x)] = g(x ) + i ^ (B-11) 

. 9Xj^2 Xj^ 

v;lth 

2 

Substituting [g(x)] for g(x) above 

E[g(x)2] = g(x^)2 + E [g a 2 (b_13) 

k 9x, ^^k ^k 

k 

The variance 



var[g(x)] = E[g(x)^] - E[g(x)]^ 



S(2Lo^^ + ^ ^ ^x ^ 

^ k 9xj^^ '’^k ^k 



g(Xo)' S(^) ^ ^ i C ^ -X 

^ ^ k 9x^^ ’^k ^ k 9Xj^^ -X 



(D-l'l) 
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The last term is neglected since it involves fourth order 
terms . Thus 

var[g(x)] E (|f-)2 ^ ^ E 

k ^k k 

Similarly the product g(x)*h(2c) is expanded about Xq to give 



Hr % 

k k 



(B-15) 



E[g(x)h(x)] ~ g(^)h(^) t I E (x-Xq)'^[||^](x-^) (B~16) 

^ J 



The matrix ~J has terms of the form 

SXfX. 

a = , l!h__ + 3^ . 3h_ + 3h_ . 8 £. + h 

Sij 8 3^^^. 3^^ 3,.. 3^^ 3^. 3^^^. 



(B-17) 



Again using the independence condition 



E[g(x)h(x)] s: 



g(x^)h(x^) + I £ [g ^ 

k 9x, 



+ 2 + 



3Xk 3Xj^ 



h 



The covariance is 



9-' 

9^k h 



(B-18) 



cov[g(x)h( x) ] = E[g(x)h(x)] - E[g(x) ]E [h (x) ] 

_ r /• s, / N . 1 ^ 3^h ^ o 3g 3h , 9^g 



[c(2, 



><o>h(2io) ■''I ^ [8 ^ * >3 X ‘ 



^ ( E ^ 0^ “T % 

k 9x^^ k .1 3Xj ^.) 



(B-19) 
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Neglecting the fourth order term, the resulting covariance 
Is 



cov[g(x)h(x) = E 

k 






k 






dx 



k 



X 



k 



- V ( ^ V 

^ ^ y hx ^x 

k '^^k ^k ^^k ^k 



(B- 20) 



The above expressions for variance and covariance were used 
to establish the Initial covariance matrix. Let 

2 

x^ = Af with variance 2 , 

2 

X- = A6 v;ith variance 2 a„ , 

2 6 

X, = V y v;lth estimated accuracy given by a 
o s X 2 

variance term a , 

^sl 

2 

and X|^ = v;lth variance Og 



Each of the partial derivatives were evaluated at the 
measured value of Af and A0 and the assumed value of v 
The resulting terms are listed below for the 

CpS. Cpd 

filter : 



si' 



° ”cpal ‘ '' 



Af 



+ 9 



A6 



si 



(B-21) 



Af 



A0 



V 



si 
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^ 2 

" ^^cpal^ " ^cpal^^^ ■’" 



.- 2 , 



(R T - ?X T 
cpal cpal 



2') 2 ^A9 „ 

^ 'a 0 2 cpal 



^ "''si" 



V 



si 



var(v^j) = 



sl 



var(e3) = ^ - ^S£al_ ^ 

^cpal 



a.f^ c.q 2 y2 

Af , A6 , si T 
— ^ ^ + 5— J 



Af" A6‘ 



V 



si 



2 2 2 

„ , V/E'^sl'^cpal ^2 r^Af . ^A0 

var(P -.) = (-- — 5 — :;z ) [ — 5- + — 5- + 

^p-^o'^cpal Af 2 A 02 



X 2 ' 

(_cp^ _ 1) __sl 



R 



cpal 



V 



si 



where 



and 



KJ. 

^AVE " KJ 



2 2 2 
R = R T •'• X r,oT 

o cpal cpal 



The covariance terms are 

A-R 



=“'''^’’cpal'’''opaI^ ° 1( 



cpal 



cpal 

2 



[X 2 (2 ^ + 6 
L^^cpal .^2 ,r.2 ’ 



Af‘ 
2 a.. 2 



A0‘ 



R 2 !m 1 + 3 !a^ + 

^cpal ..2 + 3 ^^2 ' 



-)] , 



Af 



A0‘ 



V 



(B-22) 



(B-23) 



(B-24) 



(B-25) 



(B-26) 
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with A, the angular rotation parameter 



cov(R T-v t) = -A«R T 
cpal si cpal 



si 



si 



(B-27) 



R 2 ■ 2 2 0 ,2 

^°^’^^cpal®sl^ " " 3 7^ -^-) (B-28) 



X T Af 

cpal 



A6 V 



si 



P • V • R 

ro-rr n 1 - rr 



2(2 3^ 



cpal ^^2 



A6' 



i^ 

+ —^) - X 



• V 



si 



a "^7 



cpal 



V 



si 



(B-29) 






V _.R T-‘ 

SI cpal 



X 



cpal 



"v 

si 



si 



(B-30) 



=°''[’'opaiesI^ 



A-R 



cpal 



^cpal2 



TR 2 (Z^ 
^^cpal \^2 



^2 0," 
^A9 . ^sl ^ 

O ~ O 



Af A6‘ 

„ 2 2 
X 2 , ZAL. + 2 ] 

^cpal ^ .^2 ^ ..2 ^ 



si 



(B-31) 



Af 



A0‘ 



r,. ra ^AVE’^cpal^’^sI 2,°Af . ^ 

cov[X„^,,T-P^T-J ^ 5 LX^^^t (— ^ + 2 



'^^"S=I-^opa7-''p 



cpal '^^2 



A6^ 



0 2 
^sl 
2 

'^^sl 



) - R 



cpal 



« 2 ^2 0^ 2 

2 , ^Af , ^AO . si 

( ~Y- ■'■ ~ 2“^ ^ 

Af AO"^ v_.r 



(B-32) 
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V • A • R ^ 

= -I— 



cpal 



V 



si 



2 o 2 

r = T . Si ■‘‘AVE 2 „ 2 ^ si 

cov[v jPqi] ^^cpal '^cpal ' 2 

''p'*^o-’'cpaI ''si 



-n-m PI- '^■’’■cpal'^AVE"'sl 2 °^''sl 

“ “r:,"':'; — 2 — '^’'opai • — f 



''p-’^o'^'cpal 



V 



si 



R 



rv 2 „ 2 0 „ 2 
2, ^Af , °A6 ^sl • 

2 ' 2 
''si 



cpal " ^^2 



-)] 



(B-33) 



(B-3^) 



(B-35) 



139 



APPENDIX C 



The Probabilities Associated with Measurement 
Errors for Different Values of Angle Differences 



Assume that the angle measurements are Independent samples 
consisting of the true angle plus additive white Gaussian 
noise. That is 



8i ' «lo “l 



The probability density function of the w^^'s is zero- mean 

2 

with variance • Thus each 0. is normally distributed 

U 1 

2 

with mean 0. and variance . The probability distribution 
1 0 u 

of the difference between 0. and 0., A0, is therefore 

2 

normally distributed with mean Qj^o ~ ®jo variance 20 ^ '. 

The measurement error 



0 

e 



(e,-6j) 



(e 



lo“®jo 



) 



is, as a result, normally distributed with zero mean and 
2 

variance 2o^ . From the probability tables 

U 




' “ ’’a 

e 



with P the probability associated with a normal random 

3. 

variable being within standard deviations of the mean. 
Substituting the angle variance gives 




I'lO 



If the angle difference A6 is greater than 3 , then 

Vl9j " ’’a • 



This means that 


for example 




< .5 A0} = 


Where 

a 

3 


= .5 


or a 


= 1.06 , 


The probability 


that a normal random variable is within 1.06 



standard deviations of its mean is 0.71. Thus for A6 > 3 <^d 

U 



the probability 


that 0g./A6 < .5 is greater than 0.71. 


Figure Cl shows 


a general graph of 


p {le 1 
r ' e ' 


av^T ae, „ p 

- b a 


with A0 


, , , a / 2 

- ho Q 9 and a’ = ^ 



as a parameter. 




Fig. C-1. Probability of Angie Difference Error 
for Different Values of Angle 
Difference. 



1^12 



XX 



1 



I 



% 

5 ! 
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APPENDIX D 



The Confidence Interval for Statistics 
Calculated using 200 Monte Carlo Runs 

Bucy [11] gives an approximate method for estimating the 
accuracy of computed statistics based on the assumption that 
the samples are from a normally distributed random, variable. 
The sample mean is given by equation (5.-1 ) and is 

- 1 N 

X = i E X. , (D-1) 

1=1 ^ 

where N is the number of Monte Carlo runs. The sample 
variance is 



S 




1 

N-1 



N 

Z 

i=l 




(D-2) 



p 

Let y and a be the true mean and variance of the distribution 
that the samples are from, and the quantities that are of 
Interest. The sample mean and variance are unbiasedj that is 



E(x) = y and (D-3) 

E(S^^) = . (D-il) 



The variance of the mean estimate is given by 

/-N 2 n 2 ^^^i 

var(x) ■= a— = h(x--y) ” 



E(x,^) - y^ ^2 



N 



N 



(D-5) 









?fFf 

ftfrUC 












The variance of the variance estimate Is given by 



var(S^^) = E[S^^ - a^] = 



E(x^^) - (a^)^ 
N 



(D-6) 



If the distribution of the x^^’s are approximately normal, 
then 

E(x^^) 3(a^)^ , (D-7) 

0 f 2x2 

Cg 2^ = 2 — ^ . (D-8) 

n 

_ 2 

Prom the variances for x and and probability tables, the 

_ 2 

confidence region for x and ' can be found. Let N be 2f)0 
Monte Carlo runs. Then 



and thus 

var(S^^) = 



P^{|x-yl < .1 S^} = P^{lx-y| < a 0 - } (D-9) 

S 

where 0 — = —2— ^ , and a is the number of standard 

^ v/r /T 

deviations in .1 . That is 



a 





or 



a = .1 J~200 = l.'ll'< . 



I'Ml 



The probability that a normal random variable Is within 
l.^li{ o of Its mean Is .8^, which means 



P^{|x-y| < .1 S^} = .8H 



(D-10) 



For the variance 



< 2 a, 2) = -95 . 



(D-11) 



Prom equation (D-8) 



2 a 0=2 

S ^ 
n 



fX 

\l N 



Substituting this into equation (D-11) and v/rltlng out the 
absolute value inequality gives 



P , 2vrr .2 . . 2 2 . 2 yX 2, _ 



or 



P { 

r ^ 



n 



1 + 2 



< < 



n 



■} = .95 



1-2 



N 



With N = 200 this gives 



Pr{.77^ Sn^ < a^ < 1.39 = .95 , 



or 



Pp{.88 Sj^ < o < 1.18) = .95 
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LIST OP SYSTEM LIBRARY SUBROUTINES USED 



GMPRO 


Matrix Product 


ATAN 


Arc Tangent 


MINV 


Matrix Inverse 


ARSIN 


Arc Sine 


GAUSS 


Normal Random Number Generator 


PLOTP 


Printer Plotting Package 


ARGOS 


Arc Cosine 


SIN, COS, TAN 


Trig Functions 


GMTRA 


Matrix Transpose 


SORT 


Square Root 



SORT 
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LIST OP PROGRAM VARIABLES COMMON TO BOTH PILTERS 



R(I,J) 

PD(I,J) , 
PDDTd, J) 

BR( ,J) 

VR(I,J) , 

VRD T(I,J) 

XS(J), YS(J) 

X(I), y(i) 

T(J) 

XS, YS 

VSS 

VS 

THTA 

THTAS 

OVX 

OVY 

PREQ 

VI'IED, VP 
SA , AMA 

SP, AMP 

IXA, IXP 
AVEVA, VARVA 



exact range at time T(J) from sensor I 
to target 

exact doppler shifted frequency and rate 
of change of frequency at time T(J) at 
sensor I 

exact bearing angle to target at time T(J) 
from sensor I 

radial velocity and time rate of change 
of radial velocity at time T(J) at sensor I 

exact X,Y position of target at time T(J) 

XjY position of sensor I 

time of observation 

initial X,Y position of target 

exact target velocity in knots 

VSS in yds/sec 

exact target heading in degrees 

THTA in radians 

exact target X velocity 

exact target Y velocity 

exact rest frequency 

velocity of propagation 

standard deviation and mean for normal 
angle noise generator- 

standard deviation and mean for normal 
frequency generator- 

starting numbers for noise generator 

cornpiatcd mean and variance of random angle 
noise 






I 



NSPOT 


number of different tracks to be run 


JJQQ 


number of runs in Monte Carlo simulation 


NUM 


number of time points 


TINT 


time interval between data points 


M 


number of observation variables 


Z(J,N) 


noisy observations at time T(J), Index N 
determines whether it is an angle or 
frequency and ’will depend, on number and 
type of sensors 


AVSK 


initial guess at target velocity in knots 


AVS 


AVSK in yds/sec 


PO 


initial guess at rest frequency - used only 
to determine whether target is up or down 
doppler 


KJ 


counter which Indicates how many time cuts 
were needed to satisfy angle difference 
criterion 


ADELTH 


angle difference which satisfies criterion, 
l.e., ADELTH > 3SA 


ADELT 


time difference corresponding to angle 
difference 


ATHALF 


time at which initial filter states are 
found 


ADELP 


frequency difference corresponding to 
angle difference 


ATHETH 


average of all angle measurements during 
initial condition calculation 


PAVE 


average of frequency measurements during 
initial condition calculation 


ASINE 


Arc sine of angle difference betvjeen the 
the target heading and target bearing 


ARHALP 


calculated initial target range 


FF 


one standard deviation of ADELP, equals ?. 



AA 

W 

AVAV 

HEAD (ADELF, 
ADELTH, AVS, 
ATHETH) 

HUP 

HDF 

HVA 

HDA 

HUV 

HDV 

NO 

COVK 

COV 

PHI 

PHITR 

SIGTH 

SIGVS 

SIGFO 

P(M) 

QEXIT 

HOB 

HOBTR 

GAIN 

GAIMTR 

ZNOIS 



one standard deviation of ADELTH, equals 2 
SA 

guess at accuracy of AVSK, equals 3 KTS 

one standard deviation of ATHETH, equals 
SA/ KJ 

function subprogram to calculate target 
heading from initial measurements 

target heading v^hen ADELF Is increased by PP 

target heading when ADELF is decreased by FP 

target heading when ADELTH is increased by AA 

target heading when ADELTH is decreased by AA 

target heading when AVS is Increased by VT 

target heading v;hen AVS is decreased by W 

= 0 for full initial covariance matrix, 

= 1 for only diagonal terms 

initial state covariance matrix, also updated 
state covariance matrix 

predicted state covariance matrix 

state transition matrix 

PHI transposed 

2 

heading angle excitation in (rad/K sec) 

2 

target speed excitation in ( (yds/sec)/sec ) 

2 

target rest frequency excitation (H^/Ksec) 

predicted doppler shifted frequency at sensor N 

state excitation matrix 

linearized observation matrix 

HOB transposed 

Kalman Filter gain matrix 

GAIN transposed 

observation noise covarjanco niati-lx 
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■XCUR 



■XCUR 


state vector before observation 


XPRED 


predicted state vector ( = PHI -XCUR) 


XPIL 


filtered state vector after observation 


IRE 


residual error counter 


XMC(I,J) 


Monte Carlo mean of state I for time T(J) 


XVAR(I,J) 


Monte Carlo variance of state I for time T(J) 
(See RCPA - XCPA filter) 


XTAR(J) 


filtered X position of target at time T(J) 


YTAR(J-) 


filtered Y position of target at time T(J) 


XTNT(Z) ,YINT(Z) 


average of X and Y initial positions for 
Monte Carlo runs 



RPIL(J) ,THFIL( J) calculated target range and bearing from 

filtered states at time T(J) 
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LIST OF PROGRAM VARIABLES FOR X-Y FILTER 



X(l) 


X position 


X(2) 


X velocity 


X(3) 


Y position 


x(iO 


Y velocity 


X(5) 


rest frequency 


XUF 


X refers to I'/hich state, U refers to up or 
increase, and F refers to which variable is 
changed. F is frequency term ADELF. Thus 
XUF is the calculated X initial position 
due to an increase in ADELF by FF 


XDF 


calculated X initial position due to a 
decrease in ADELF by PF 


SGXP 


average of the change due to increasing 
ADELF and the change due bo decreasing ADELF 
by AA 


SGVXA 


average of the change due to increasing 
ADELTH and the change due to decreasing 
ADELTH by A A 



The other terms follov; the same pattern 



TT 


time interval between measurements 


SNEWXV, 

SNEWYV, 


SMEV/XS, Monte Carlo variances and standard deviations 
SNEV/YS of X and Y positions in rotated coordinate 
system 


HDING 


calculated initial target heading 


HI 


predicted target bearing from sensor 1 in 
radians 


RESA 


angle residual for sensor 1 in radians 


RESFK 


frequency residual for sensor K in 



I'jl 



RERROR(J) 


true range error for time T(J) i.e. distance 
from filtered position to true position 


VSPIL(J) 


computed target speed from filtered X and 
Y velocities at time T(J) 


HDPIL(J) 


computed target heading from filtered X 
and Y velocities at time T(J) 


RTEMP 


computed target range from filtered X and 
Y positions 


XYVAR(I,J) 


Monte Carlo covariance of X and Y filter 
positions at time T(J) 


XYVAR(Z,J) 


Monte Carlo covariance of X and Y filter 
velocities at time T(J) 


XYA(J) 


angle of Monte Carlo probability ellipse 
for position variances 


VXVYA(J) 


angle of Monte Carlo probability ellipse 
for velocity variances 
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LIST OF PROGRAM VARIABLES FOR R , X FILTER 
cpa^ — cpa 



X(l) 


range to target at CPA from sensor' 1 


X(2) 


range of target to CPA 


X(3) 


target speed 


x(^0 


•target heading 


X(5) 


rest frequency 


REX 


exact value of Range to CPA from sensor 1 


XEX 


exact value of Range to CPA from target 


NDIPA 


number of angle and frequency sensors 


NLOFA 


number of frequency sensors 


NBU 


total number of sensors used 


W 


angular rotation parameter around sensor 1 
(~ +1 counterclockwise 5 = -1 clockvnlse) 


RCPAI 


initial filter state X(l) 


XCPAI 


initial filter state X(2) 



Initial covariance terms are defined in a similar manner as 
the X-Y filter. Thus RW is the initial oue to an 

Increase in Avs by W. 



DELTAT 


time Interval between measurements 


D(I) 


distance from sensor 1 to sensor I 


TH(I) 


bearing from sensor 1 to sensor I 


RCPA(I) 


predicted range at CPA fro sensor I 


XCPA(I) 


predicted range to CPA of target for sensor I 


RANGE ( I ) 


predicted range from target to sensor I 


THTAX(I) 


predicted bearing of target from sensor I 


RES 1 


angle residual for sensor 1 
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RES 2 

RES 3, RES k 

AVRESA 

STDRAZ 

AVRPl 

STFIZ 

ICOUNT 

lEND 

XlviCdjJ), 

XMC(Z,J) 

XVAR(I,J) 3 
XVAR(Z,J) 

XYVARd, J) 
XYVAR(Z,J) 



frequency residual far sensor 1 

angle and frequency residual depending on 
number and type of sensors 

statistical mean of RESI for a single run 

statistical standard deviation of RESI for 
a single run 

statistical mean of RES2 for a single run 

statistical variance of RES2 for a single 
run 

number of times filter has been reinitialized 
due to residual error counter IRE 

maximum number of times a- reinitialization 
will be tried 

Monte Carlo mean of target X and Y position 
at tlm.e T(J) 

Monte Carlo variance of target X and Y 
position at time T(J) 

Monte Carlo covariance of X and Y position 
at time T(J) 

Monte Carlo covariance of target speed and 
heading at time T(J) 



Flow Chart for x-y 
Filter Simulation Program 
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1 



Flow Char! for x-Y Filter, 
F liter Program 
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